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ABSTRACT 


This  research  concerns  the  development  of  cooperative  control  of  two  spacecraft 
mounted  two-link  manipulators  as  they  reposition  a  common  payload.  Lagrangian  formu¬ 
lation  is  used  to  determine  the  system  equations  of  motion  Lyapunov  stability  theory  is 
used  to  develop  the  cooperative  control  by  using  a  reference  trajectory  and  reference  actu¬ 
ator  torques.  Polynomial  curves  represent  potential  reference  trajectories.  Numerical 
methods  select  specific  reference  trajectories  to  minimize  the  disturbance  torque  transmit¬ 
ted  to  the  spacecraft  during  the  payload  repositioning  maneuver.  The  reference  actuator 
torques  are  selected  to  minimize  weighted  norms  of  the  torques.  Analytical  and  experi¬ 
mental  models  of  planar  motion  are  used  to  study  the  performance  of  different  cooperative 
controllers.  The  fifth  order  polynomial  reference  trajectory  leads  to  superior  performance 
in  terms  of  spacecraft  attitude  accuracy,  actuator  torque  magnitude,  payload  repositioning 
accuracy,  and  maneuver  time.  The  higher  order  polynomial  reference  trajectory  results  in 
only  minor  improvement  in  performance.  The  experimental  results  verify  the  concept  of 
cooperative  control. 


m 


Actc '■>?:'  I  on  lor 


:xi 


1 

r 


ACKNOWLEDGMENTS 


This  work  would  not  have  been  possible  without  the  contributions  from  many  people. 
Mr.  RatYord  Bailey  designed  the  manipulators  and  toiled  numerous  hours  in  AutoCad  doc¬ 
umenting  the  design  in  drawings.  Mr.  John  Moulton  converted  the  drawings  into  expertly 
crafted  components.  Professor  John  L.  Junkins.  the  George  J  Eppright  Chair  Professor  at 
Texas  A&M  University  and  visiting  Co-Chair  of  the  Space  Systems  Academic  Group  at 
the  Naval  Postgraduate  School  suggested  the  general  scope  of  this  work  and  ensured  suc¬ 
cessful  development  of  a  fixed-base  version.  Dr.  Hyochoong  Bang  provided  significant 
assistance  with  the  theoretical  work.  In  addition  to  his  guidance  regarding  the  analytical 
and  experimental  work.  Professor  Agrawal  was  a  constant  source  of  encouragement. 
Finally,  the  author’s  family  supplied  a  tremendous  amount  of  support  for  which  he  is 
deeply  grateful. 


iv 


TABLE  OF  CONTENTS 


I.  INTRODUCTION  .  1 

A.  BACKGROUND  AND  LITERATURE  SURVEY  .  1 

B  DISSERTATION  OVERVIEW  AND  OBJECTIVES  . 5 

II  ANALYTICAL  MODEL  . 7 

A  COORDINATE  SYSTEMS  . 7 

B  EQUATIONS  OF  MOTION  . 9 

1 .  Inertia  Matrix,  M .  11 

2.  Centripetal  and  Coriolis  Matrix,  G  .  17 

3.  Generalized  Forces,  Q .  19 

4.  Constraints  Matrix,  A  . 21 

C  SIMPLIFIED  EQUATIONS  OF  MOTION  . 23 

D.  REFERENCE  TORQUES  . 24 

E.  LYAPUNOV  CONTROLLER  . 26 

F.  REFERENCE  TRAJECTORIES  . 29 

1  Calculating  Redundant  Coordinates . 29 

2.  Selecting  Reference  Trajectories . 35 

III.  VALIDATION  AND  SIMULATION  RESULTS . 45 

A.  VALIDATION . 45 

1.  Conservation  of  Kinetic  Energy  . 45 

2.  Conservation  of  Angular  Momentum  . 52 

3.  Wheel  Torque  and  Constraints  . 58 

B.  SIMULATIONS  . 60 


v 


1 .  Lyapunov  Point  Controller . 60 

2.  Lyapunov  Tracking  Controller . 65 

3  Modified  Lyapunov  Tracking  Controller . 72 

4.  Comparison  of  Controllers . 75 

IV  EXPERIMENTAL  WORK . 77 

A.  SETUP  . 77 

1 .  Centerbodv  . 79 

2.  Manipulators  . 80 

3.  Payload . 83 

4.  Controller . 83 

5.  System  Integration  . 84 

B.  RESULTS  . 86 

V  SUMMARY  AND  CONCLUSIONS  . 93 

A.  SUMMARY  . 93 

B  ORIGINAL  CONTRIBUTIONS . 94 

C  RECOMMENDATIONS  FOR  FUR!  TIER  STUDY  . 95 

APPENDIX  A:  EXPERIMENTAL  CONTROL  BLOCK  DIAGRAMS . 97 

APPENDIX  B:  MATLAB  CODE . 109 

A.  AngMo  .  110 

B.  AngMo2  .  112 

C.  Draw3 .  114 

D.  Eqn2  .  116 

E.  fminu2 . 121 

F.  MainMin  .  126 

G.  MainOpt . 129 

H.  Main2  .  131 

vi 


I  Matx 


142 


J.  MatxFix .  146 

K.  Ref2 .  150 

L.  RefMin2  .  158 

REFERENCES  .  165 

INITIAL  DISTRIBUTION  LIST  .  167 


vii 


LIST  OF  FIGURES 


Figure  1:  Dual  Two-Link  Manipulator  Configuration  . 8 

Figure  2:  Deriving  Left  Manipulator  Joint  Angles  . 30 

Figure  3:  Deriving  Right  Manipulator  Joint  Angles  . 32 

Figure  4  Test  Case  1  Angles  . 47 

Figure  5:  Test  Case  1  Angular  Rates . 47 

Figure  6:  Test  Case  1  Time  Lapse  Stick  Figure  . 48 

Figure  7:  Test  Case  1  Kinetic  Energy  . 48 

Figure  8:  Test  Case  1  Angular  Momentum  . 49 

Figure  9:  Test  Case  2  Angles  . .50 

Figure  10:  Test  Case  2  Angular  Rates . 50 

Figure  1 1 :  Test  Case  2  Time  Lapse  Stick  Figure  . 51 

Figure  12:  Test  Case  2  Kinetic  Energy  . 51 

Figure  13:  Test  Case  2  Angular  Momentum  . 52 

Figure  14:  Test  Case  3  Angles  . 53 

Figure  15:  Test  Case  3  Angular  Rates . 54 

Figure  16:  Test  Case  3  Time  Lapse  Stick  Figure  . 54 

Figure  17:  Test  Case  3  Kinetic  Energy  . 55 

Figure  18:  Test  Case  3  Angular  Momentum  . 55 

Figure  19:  Test  Case  4  Angles  . 56 

Figure  20:  Test  Case  4  Angular  Rates . 56 

Figure  21 :  Test  Case  4  Time  Lapse  Stick  Figure  . 57 

viii 


Figure  22:  Test  Case  4  Kinetic  Energy  . 57 

Figure  23:  Test  Case  4  Angular  Momentum  . 58 

Figure  24:  ample  of  Wheel  Torque  and  Change  in  Angular  Momentum  vs  Time  59 

Figure  25  Sample  of  Constraints  vs  Time  . 60 

Figure  26:  Desired  Repositioning  Maneuver  . 61 

Figure  27:  Lyapunov  Point  Controller  Angles  . 62 

Figure  28:  Lyapunov  Point  Controller  Displacement  Errors  . 62 

Figure  29:  Lyapunov  Point  Controller  Angular  Rates  . 63 

Figure  30  Lyapunov  Point  Controller  Command  Torques . 63 

Figure  3 1 :  Lyapunov  Point  Controller  Time  Lapse  Stick  Figure . 64 

Figure  32:  Lyapunov  Point  Controller  Initial  and  Final  Stick  Figures . 64 

Figure  33:  5th  Order  Reference  Trajectories . 66 

Figure  34:  5th  Order  Angles . 66 

Figure  35:  5th  Order  Displacement  Errors  . 67 

Figure  36  5th  Order  Angular  Rates  . 67 

Figure  37:  5th  Order  Command  Torques  . 68 

Figure  38:  5th  Order  Time  Lapse  Stick  Figure  . 68 

Figure  39:  8th  Order  Reference  Trajectories . 70 

Figure  40:  8th  Order  Angies . 70 

Figure  41:  8th  Order  Angular  Rates  . 71 

Figure  42.  8th  Order  Command  Torques  . 71 

Figure  43:  8th  Order  Time  Lapse  Stick  Figure  . 72 

Figure  44:  Modifttd  Lyapunov  Tracking  Controller  Angles  . 73 

Figure  45:  Modified  Lyapunov  Tracking  Controller  Displacement  Errors  . 73 


ix 


Figure  46:  Modified  Lyapunov  Tracking  Controller  Angular  Rates  . 74 

Figure  47:  Modified  Lyapunov  Tracking  Controller  Command  Torques . 74 

Figure  48:  Modified  Lyapunov  Tracking  Controller  Time  Lapse  Stick  Figure  75 

Figure  49:  Spacecraft  Robotics  Simulator  78 

Figure  50:  System  Top  View . 79 

Figure  51:  Left  Manipulator  Top  and  Side  Views  . 81 

Figure  52:  Right  Manipulator  Top  and  Side  Views  . 82 

Figure  53:  Desired  Experimental  Repositioning  Maneuver . 86 

Figure  54:  0p  Commanded,  Actual,  and  Error  Angles  vs  Time . 87 

Figure  55:  0[  j  Commanded,  Actual,  and  Error  Angles  vs  Time  . 88 

Figure  56:  0[  2  Commanded,  Actual,  and  Error  Angles  vs  Time  . 89 

Figure  57:  0R)  Commanded,  Actual,  and  Error  Angles  vs  Time . 90 

Figure  58:  0K2  Commanded,  Actual,  and  Error  Angles  vs  Time . 91 

Figure  59:  Super  Blocks  Hierarchy  . 97 

Figure  60:  Overall  Control  Block  Diagram  . 99 

Figure  61:  Parameters  Block  Diagram  . 100 

Figure  62:  Reference  Trajectory  Block  Diagram  . 101 

Figure  63:  Encoders  Block  Diagram  . 102 

Figure  64:  Left  Angles  Block  Diagram  . 103 

Figure  65:  Part  1  Block  Diagram  . 104 

Figure  66:  Part  2  Block  Diagram  . 105 

Figure  67:  Part  3  Block  Diagram  . 106 

Figure  68:  Right  Manipulator  Controller  Block  Diagram . 107 

Figure  69:  Left  Manipulator  Controller  Block  Diagram  . 108 


Figure  70:  MATLAB  Modules  Hierarchy 


109 


xi 


LIST  OF  TABLES 


TABLE  1 .  GENERIC  MODEL  SYSTEM  PROPERTIES . 46 

TABLE  2.  COMPARISON  OF  HYPOTHETICAL  MODEL  SIMULATIONS . 76 

TABLE  3.  MOMENTUM  WHEEL  MOTOR  CHARACTERISTICS . 80 

TABLE  4.  MANIPULATOR  ACTUATOR  CHARACTERISTICS . 82 

TABLE  5.  POWER  SUPPLIES  CHARACTERISTICS . 83 

TABLE  6.  EXPERIMENTAL  ERROR  ANGLES . 92 


I.  INTRODUCTION 


A.  BACKGROUND  AND  LITERATURE  SURVEY 

Robots  are  presently  an  integral  part  of  industrial  processes.  They  perform  tasks  with 
high  precision,  speed  and  reliability.  These  same  features  make  robots  attractive  with 
regards  to  space  applications. 

Space  based  robotics  platforms  experience  conditions  unlike  those  of  their  terrestrial 
counterparts.  With  respect  to  the  dynamics  of  the  systems,  the  most  notable  difference  is 
the  absence  of  a  fixed  base  on  which  to  locate  the  manipulators.  The  consequence  of  the 
difference  is  that  the  motion  of  the  space  based  manipulator  transmits  forces  and  moments 
to  its  mounting  base  resulting  in  translation  and  rotation  of  the  base  itself.  This  of  course 
impacts  the  location  of  the  manipulator’s  end  effector.  The  problem  is  further  complicated 
in  that  the  disturbances  are  not  simply  a  function  of  the  present  manipulator  joint  angles 
but  are  also  a  function  of  the  joint  angle  histories  preceding  the  current  configuration. 
(Ref.  1) 

A  number  of  approaches  have  been  used  for  dealing  with  this  coupling  of  joint  angle 
histories  and  spacecraft  main  body  attitude.  Wang  (Ref.  2)  eliminates  the  problem  by 
carefully  defining  what  he  expects  of  his  dual-arm  maneuverable  space  robot.  He  preposi¬ 
tions  the  manipulators  such  that  they  are  configured  to  grasp  the  payload  once  the  vehicle 
moves  within  range.  After  the  manipulators  are  in  position,  their  joints  are  locked  while 
the  spacecraft  maneuvers  to  a  location  and  attitude  near  the  payload.  Next,  the  vehicle 
approaches  the  payload  in  a  straight  line  until  the  end  effectors  can  grasp  the  payload. 
While  the  manipulator  joints  remain  locked,  the  vehicle  repositions  the  entire  rigid  body 
system  to  the  desired  payload  destination.  At  this  point,  the  payload  is  released  and  the 


vehicle  backs  away  along  a  straight  line.  The  repositioning  of  the  payload  is  accom¬ 
plished  by  means  of  the  vehicle  attitude  control  rather  than  altering  the  joint  angles  in  the 
manipulators.  The  manipulators  themselves  are  not  moved  except  when  the  attitude  dis¬ 
turbance  they  impart  to  the  vehicle  is  of  no  importance.  Maintaining  vehicle  attitude  dur¬ 
ing  manipulator  motion  is  not  a  requirement. 

Longman,  Lindberg  and  Zedd  (Ref.  3)  calculate  the  disturbance  torques  caused  by 
manipulator  motion.  This  information  is  used  to  calculate  reaction  wheel  commands 
which  will  compensate  for  the  disturbance  torques.  In  this  way,  spacecraft  attitude  control 
is  maintained  while  the  manipulator  is  repositioned. 

If  the  vehicle  does  not  contain  reaction  wheels,  the  primary  source  of  attitude  control 
is  probably  reaction  control  thrusters.  Because  fuel  is  consumable  and  hence  mission  lim¬ 
iting,  firing  thrusters  to  hold  spacecraft  attitude  should  be  avoided  whenever  possible. 
Vafa  and  Dubowsky  (Ref.  4)  and  Longman  (Ref.  5)  use  similar  approaches  to  eliminate 
the  need  for  reaction  thruster  firings.  Both  techniques  involve  constructing  a  manipulator 
trajectory  which  involves  revolving  the  manipulator  in  a  small  coning  motion  at  interme¬ 
diate  stages  of  the  payload  repositioning  maneuver.  This  motion  imparts  a  slow  rotation 
of  the  spacecraft  about  the  coning  axis.  Careful  use  of  the  coning  locations  permits  repo¬ 
sitioning  of  the  payload  between  any  arbitrary  locations  (within  manipulator  reach)  and 
attitudes  while  also  changing  the  spacecraft  to  any  desired  attitude  without  the  need  for 
thruster  firings.  This  technique  does  not,  however,  maintain  a  particular  spacecraft  atti¬ 
tude  during  the  maneuver. 

Nakamura  and  Mukherjee  (Ref.  6)  use  a  technique  called  the  bi-directional  approach. 
This  method  represents  a  six  degree  of  freedom  (DOF)  manipulator  mounted  on  a  space 
vehicle  as  a  nine  variable  system  (six  joint  angles  and  three  spacecraft  attitude  angles) 
with  six  inputs  (the  manipulator  joint  torques).  They  attack  the  problem  from  both  ends 
by  integrating  forward  from  the  initial  conditions  and  backwards  from  the  desired  final 


2 


conditions.  A  Lyapunov  function  guarantees  that  the  two  solutions  will  converge  at  some 
intermediate  time  during  the  maneuver  As  in  Ref  4  and  Ref.  5,  the  payload  is  reposi¬ 
tioned  to  the  desired  location  and  attitude  and  the  attitude  of  the  spacecraft  main  body  is 
changed  to  its  desired  orientation.  However,  the  main  body  attitude  during  the  maneuver 
is  not  controlled.  Furthermore,  the  joint  angle  trajectories  calculated  by  this  method  con¬ 
tain  rapid,  large  oscillations  near  the  maneuver  start  and  stop  times  and  noncontinuous 
derivatives  at  the  convergence  time. 

Vafa  (Ref.  7)  succeeds  in  using  a  single  space-based  manipulator  to  control  spacecraft 
attitude  during  a  repositioning  maneuver.  He  does  this  by  employing  a  nine  DOF  manipu¬ 
lator.  This  manipulator  has  enough  redundancy  in  its  kinematics  to  control  the  end  effec¬ 
tor  location  and  attitude  and  the  spacecraft  attitude.  Six  DOF  are  allocated  to 
repositioning  the  payload  and  the  remaining  three  DOF  are  used  to  control  the  spacecraft 
attitude. 

Like  Ref.  7,  the  primary  objective  of  Chung,  Desa  and  deSilva  (Ref  8)  is  to  address 
the  disturbances  transmitted  to  the  spacecraft  by  the  manipulator  motion.  They  also  use  a 
single  manipulator  with  redundant  kinematics.  Because  they  use  inverse  kinematics  to 
find  the  joint  torques,  the  manipulator  redundancy  prevents  the  existence  of  a  unique  solu¬ 
tion.  A  solution  is  selected  from  among  the  infinity  of  possible  solutions  by  means  of 
minimizing  a  cost  function  of  the  magnitudes  of  the  forces  and  torques  transmitted  to  the 
base. 

Torres  and  Dubowsky  (Ref.  9)  also  focus  on  the  spacecraft  attitude  disturbances 
caused  by  manipulator  motion.  They  recognize  that  for  any  given  point  in  joint  space, 
there  is  a  direction  of  motion  which  produces  minimum  spacecraft  attitude  disturbance 
and  a  perpendicular  direction  of  motion  which  produces  maximum  spacecraft  attitude  dis¬ 
turbance.  A  tool  called  the  enhanced  disturbance  map  (EDM)  depicts  these  directions 
graphically.  The  EDM  permits  users  to  plan  manipulator  trajectories  that  lie  on  or  near 
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zero  disturbance  paths.  For  non  redundant  manipulators,  this  may  involve  repositioning 
the  spacecraft  itself  prior  to  the  manipulator  repositioning  maneuver  If  the  manipulator 
has  redundant  kinematics,  one  can  find  a  zero  disturbance  path  to  connect  the  manipulator 
trajectory  endpoints  without  having  to  reposition  the  spacecraft  initially.  This  technique 
considers  only  the  location  of  the  manipulator  endpoint  and  not  its  attitude. 

Configurations  with  multiple  manipulators  have  also  been  explored.  The  closed  chain 
nature  of  these  configurations  prevent  the  use  of  some  of  the  techniques  already  discussed. 
In  addition,  controlling  multiple  manipulators  raises  the  issue  of  cooperation  between  the 
manipulators. 

Nguyen,  Pooran  and  Premack  (Ref.  10)  develop  a  PD  controller  for  a  fixed  base,  two 
DOF,  closed  chain  manipulator  system  The  system  is  linearized  by  means  of  Taylor 
series  expansion  about  a  point  designated  as  the  robot’s  “home"  point.  Pole  placement  is 
then  used  to  select  controller  gains. 

Hu  and  Goldenberg  (Ref.  11)  derive  an  adaptive  control  scheme  for  multiple  nonre- 
dundant  manipulators  mounted  to  a  fixed  base.  In  this  reference,  coordinated  control 
involves  controlling  the  motion  of  the  grasped  object,  the  contact  forces  between  the 
object  and  its  environment,  and  the  internal  forces  within  the  object  caused  from  being 
held  by  more  than  one  manipulator. 

For  a  space  based  system,  contact  forces  between  the  payload  and  its  environment  are 
less  likely  to  be  imponant.  Walker,  Kim  and  Dionise  (Ref.  12)  present  just  such  an  adap¬ 
tive  controller. 

Coordinated  control  of  multiple  manipulators  assumes  a  different  meaning  according 
to  Yoshida,  Kurazume  and  Umetani  (Ref.  13).  Although  they  propose  a  system  with  two 
manipulators,  only  one  actually  grasps  the  payload.  The  other  is  used  to  provide  counter¬ 
balancing  torques  to  the  spacecraft  main  body.  The  role  of  the  second  manipulator  is  sim- 
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ilar  to  a  reaction  wheel  in  that  its  primary  function  is  to  control  spacecraft  attitude  rather 
than  reposition  a  payload. 

Ahmad  and  Zribi  (Ref.  14)  apply  a  Lyapunov  controller  to  a  fixed  base,  multiple 
manipulator  system.  As  in  Ref.  12,  they  are  concerned  with  controlling  the  payload  posi¬ 
tion  and  its  internal  forces.  To  do  so,  the  method  requires  sensors  to  measure  the  forces 
and  moments  created  by  each  manipulator.  They  also  present  an  adaptive  version  to  con¬ 
trol  this  system. 

While  still  addressing  payload  position  and  internal  forces,  Schneider  and  Cannon 
(Ref.  15)  use  a  technique  called  object  impedance  control  to  achieve  coordinated  control 
among  the  manipulators.  This  method  views  the  payload  as  being  anchored  to  a  desired 
location  by  a  spring/damper  system. 

B.  DISSERTATION  OVERVIEW  AND  OBJECTIVES 

This  research  is  concerned  with  the  cooperative  control  of  a  space  based  manipulator 
system  with  multiple  manipulators  handling  a  common  payload.  The  scope  is  limited  to 
planar  motion  in  which  the  spacecraft  is  allowed  to  rotate  but  not  to  translate.  These 
restrictions  permit  experimental  verification  in  the  Spacecraft  Dynamics  and  Control  Lab¬ 
oratory  at  the  Naval  Postgraduate  School.  The  objectives  of  this  research  are  to  1 )  develop 
a  stable  control  law  which  facilitates  cooperation  among  the  manipulators  as  they  reposi¬ 
tion  the  payload,  2)  minimize  joint  actuator  effort,  3)  reduce  the  disturbance  torque  trans¬ 
mitted  to  the  spacecraft  main  body  by  the  manipulator  motion,  and  4)  validate  the 
analytical  development  with  experimental  results. 

Chapter  II  develops  the  analytical  model  in  detail.  Coordinate  systems  are  defined  and 
the  equations  of  motion  are  derived.  A  technique  for  finding  control  torques  which  mini¬ 
mizes  a  weighted  norm  is  presented.  A  globally  stable  control  law  is  developed  using 
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Lyapunov  methods.  The  idea  of  using  a  reference  trajectory  to  describe  the  motion  is 
applied  as  are  methods  for  choosing  the  reference  trajectory 

Chapter  III  verifies  the  analytical  model  with  several  test  cases.  The  model  is  evalu¬ 
ated  for  compliance  with  the  principles  of  conservation  of  kinetic  energy  and  angular 
momentum.  After  establishing  the  validity  of  the  model,  results  from  simulations  are  pre¬ 
sented.  The  stability  of  the  controller  is  illustrated  as  is  the  dramatic  improvement  in  per¬ 
formance  when  a  reference  trajectory  is  included  Results  from  a  simplified  control  law 
which  is  more  practical  to  implement  are  included  and  compared  to  the  complete  control 
law  version. 

Discussion  of  the  experimental  work  is  contained  in  Chapter  IV  This  chapter  includes 
a  description  of  the  experimental  setup.  As  might  be  expected,  actual  hardware  demon¬ 
strated  that  there  are  differences  between  the  ideal  world  of  the  analytical  model  and  the 
real  world  of  hardware  implementation. 

The  summary  and  concluding  remarks  are  presented  in  Chapter  V.  This  chapter  also 
contains  suggestions  for  future  work  in  this  field. 
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II.  ANALYTICAL  MODEL 

The  analytical  model  represents  a  spacecraft  with  two  manipulators  attached.  The 
manipulators  have  already  firmly  grasped  an  object  hereafter  referred  to  as  the  payload. 
The  manipulators  are  about  to  reposition  the  payload  with  respect  to  the  spacecraft.  The 
ensuing  dynamics  between  the  spacecraft,  manipulators,  and  payload  are  the  topic  of  this 
research.  What  occurs  before  the  manipulators  grasp  the  payload  and  after  they  release  it 
is  beyond  the  scope  of  this  investigation.  The  scope  is  narrowed  further  by  confining  the 
motion  to  be  two  dimensional  and  allowing  the  spacecraft  to  rotate  but  not  translate. 
These  assumptions  are  consistent  with  hardware  restrictions  during  experimental  verifica¬ 
tion. 

A.  COORDINATE  SYSTEMS 

Figure  1  shows  a  schematic  of  the  overall  system.  This  diagram  illustrates  the  rela¬ 
tionship  between  the  coordinate  frames  used  to  develop  the  equations  of  motion.  All 
angles  are  measured  positive  counterclockwise  The  centerbody,  manipulator  links,  and 
payload  are  assumed  to  be  rigid  bodies.  Therefore,  member  lengths  (4i,  t ^  4*2* anc* 

4X  distances  to  member  centers  of  mass  (4o.  4-j.b  4l.2*  4ri*  4r2>  ant*  4b X  and  the  loca¬ 
tion  of  the  left  and  right  shoulders  (4y,  0j,o,  and  0RO)  remain  constant.  An  inertial 
axis  system  is  located  on  the  centerbody  at  the  point  of  rotation.  A  body  fixed  coordinate 
frame  uses  the  same  origin  as  the  inertial  frame  but  rotates  with  the  spacecraft  centerbody. 
The  x-axis  of  this  frame  points  to  the  centerbody  center  of  mass.  The  centerbody  attitude, 
00,  is  the  angle  between  the  inertial  x-axis  and  the  spacecraft  centerbody  x-axis.  Each 
manipulator  link  has  its  own  set  of  body  axes.  A  coordinate  frame  attached  to  the  left 
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shoulder  aligns  its  x-axis  along  the  longitudinal  axis  of  manipulator  Link  L 1  The  attitude 
of  this  link,  0|  |,  is  zero  when  the  link  lies  on  a  ray  extending  from  the  inertial  origin 
through  the  left  shoulder.  The  attitude  of  Link  L2  is  defined  by  a  coordinate  frame 
attached  to  the  left  elbow  The  attitude  of  this  link,  0|  2,  is  zero  when  the  link  is  parallel 
with  the  proceeding  link.  Link  LI.  Similar  coordinate  frames  and  definitions  apply  to  the 
right  manipulator.  The  payload  attitude,  0p,  is  referenced  to  the  inertial  frame.  The  Carte¬ 
sian  coordinates  of  the  payload  center  of  mass  are  also  with  respect  to  the  inertial  frame. 
A  set  of  generalized  coordinates  which  describe  the  system  include  the  centerbody  atti¬ 
tude,  joint  angles  for  all  of  the  manipulator  links,  and  payload  attitude  and  position. 


y  ~  [°O0L1  ®l,2  ®RI  ®R2  Yl>!  ^ 

Six  joint  actuators  apply  torques  at  the  shoulder,  elbow,  and  wrist  of  each  manipulator. 
A  reaction  wheel  applies  a  torque  to  the  centerbody  The  actuators  can  be  grouped  into  a 
control  vector 


u  = 


Uwh  ULS  ULL  ULW  URS  URI-  URW 


(2) 


where  the  first  element  is  the  reaction  wheel  torque.  The  remaining  elements  are  joint 
actuator  torques.  The  first  letter  of  their  torque  subscripts  indicates  left  or  right  arm.  The 
second  subscript  indicates  shoulder  (S),  elbow  (E)  or  wrist  (W) 


B.  EQUATIONS  OF  MOTION 

The  equations  of  motion  for  this  system  are  developed  using  Lagrange’s  equations  for 
a  dynamic  system  with  holonomic  constraints. 

d^ 
dt 

subject  to  constraints 


'  dL 

r)n 


c  =  q  +  aJx 


Oq 


(3) 


9 


Aq  +  A()  =  0 


(4) 


where  L  =  T  -  V 

T  is  kinetic  energy 
V  is  potential  energy 
q  is  the  generalized  coordinates  vector 
q  is  the  generalized  velocities  vector 

Q  is  the  vector  of  applied  nonconservative  forces 

T 

A  X  are  the  constraint  forces 

Beginning  with  Lagrange’s  equation,  the  equations  of  motion  can  be  rearranged  into 
an  alternate  form.  The  inertia  matrix,  M,  is  a  function  of  the  generalized  coordinates. 
Since  the  potential  energy  is  a  function  of  only  the  generalized  coordinates,  the  partial  of 
the  Lagrangian  with  respect  to  the  generalized  velocities  does  not  contain  any  potential 
energy  terms 


'T.Mcj 

dq 

Differentiating  Eq.  (5)  with  respect  to  time  leads  to 


dt 


TdM  ^  ,t<JM 

=  Mq  +  q  —  =  Mq  +  q 


(5) 


(6) 


Replacing  the  Lagrangian  with  expressions  for  kinetic  energy  and  potential  energy 
results  in 


Eq.  (6)  and  Eq.  (7)  can  be  substituted  back  into  Eq.  (3)  to  produce 


Q  +  ArX 


(7) 


(8) 
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The  second  term  on  the  left-hand  side  of  Eq  (8)  contains  the  centripetal  and  Coriolis 
torques  Replacing  this  with  the  G  matrix  leads  to 

Mq  +  G(q,q)  +'’V  =  Q  +  A1?.  (9) 

rq 

After  substituting  the  matrix  form  of  the  generalized  forces  into  the  equations  of 
motion,  one  has 

rV  | 

Mq  +  G  +  =  Bu  +  A  A.  (10) 

rq 

The  following  sections  develop  expressions  for  the  inertia  matrix,  Coriolis  and  cen¬ 
tripetal  accelerations,  generalized  forces,  and  constraints  imposed  by  the  closed  chain 
geometry  of  the  system. 

(.  Inertia  Matrix,  M 

The  inertia  matrix  is  found  by  calculating  the  system  kinetic  energy  and  expressing 
it  in  the  form 


t  =  2(ir|M(q)j(i 


(H) 


The  inertia  matrix  is  the  term  bracketed  by  the  generalized  velocity  vectors.  The 
total  system  kinetic  energy  is  the  sum  of  the  kinetic  energy  of  all  the  pieces. 


T  -  TQ  +  Tl  ,  +  Tj  2  +  Tr  ,  +  TR2  +  Tp 
Kinetic  energy  of  individual  components  can  be  found  from 


(12) 


T  =  -I  to"  +  -m  (f  r)  (13) 

i  2  1  1  2  1 

Ij  is  the  member  moment  of  inertia  about  its  center  of  mass 
o>j  is  the  angular  velocity 
mj  is  the  mass 

r  is  the  inertial  velocity  of  the  center  of  mass 
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The  centerbody  angular  rate  and  center  of  mass  position  vector  are  given  by 


(14) 

r  =  /  .n.,  (15) 

Differentiating  Eq  (15)  results  in  the  velocity  of  the  centerbody  center  of  mass 

r,,  =  ^  ii  (16) 

Substituting  Eq.  ( 14)  and  Eq  (16)  back  into  the  expression  for  kinetic  energy  (Eq. 
(13))  and  collecting  on  the  angular  rate  term  leads  to 


= ,,  +  ,», (,7) 

Similar  developments  are  used  for  each  of  the  remaining  pieces  in  the  system  For 
the  left  manipulator  link  between  the  shoulder  and  elbow  (Link  L 1 ),  the  angular  velocity  is 
a  combination  of  centerbody  rotation  and  rotation  of  the  link  with  respect  to  the  center- 
body 


,  =  <»„  +  (l|  , 

The  position  vector  to  the  center  of  mass  is 

rL,  =  *1.0cosWl  <>*<>  +  *MS'"°|  n>'o  +  f;u\| 


(18) 


(19) 


The  first  two  terms  in  the  position  vector  represent  the  location  of  the  left  shoulder. 
Differentiating  the  position  vector  gives  the  expression  for  the  velocity  vector.  Because 
none  of  the  coordinate  axes  used  in  the  position  vector  expression  are  inertial,  their  rota¬ 
tion  must  be  included  as  well. 


ru  =  V,UC0S°LU><>-'|  Hi. 


II  11 


(20) 


After  Eqs.  (18)  and  (20)  are  substituted  into  Eq.  (13)  and  terms  are  grouped  by 
angular  rates,  the  kinetic  energy  is 


IL|  =  6o(O.S(Il|+mL/eL1+mL 


(21) 
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+  miAi/«u  1  s,,'0I.osi"  ("|  i.  +  ",  i*  +  c°S0|  „cos  .0,  „+<>,,))  ) 

+  ‘,-5«u«L,+ml  /;L,) 

+  Mli  <Iu  +'"l/cL1  +  ml.l*l  .A.-I  i  1  sinHItlsi"(0I  (J+«|  ,)  +cosO|  0cos(0|  0  +  ()|  ,))) 

The  angular  rate  for  the  left  forearm  includes  the  centerbody  angular  rate  as  well  as 
the  angular  rates  of  the  body  axes  on  Links  L  i  and  L2. 

<'>L,  =  bn  +  ti|  |  +()| ,  (22) 

This  link’s  center  of  mass  position  vector  is 

rU  =  £L0COsHL0^  +  Vosin".n>..+<M!tl  ,  +  *«l.:\2  (23) 

Differentiating  the  position  vector  gives  the  velocity  vector. 

'L,  =  *LO%COS%yo-*l.O<,Vi"°!  .i*"+*UWl  ,yL,  +4«|.2®I.2yu  ^24) 

The  kinetic  energy  expression  for  Link  L2  is  found  after  substituting  Eqs.  (22)  and 
(24)  into  Eq.  (13)  and  collecting  terms  with  common  angular  rates. 

Tl2  =  < 0.5  (IL;  + +  +  m,  ,^_0)  (25) 

+  mL2*L<A  I  (  si,,eL0sin  1 %  +  B|.  I  >  +  COsHU)tOS  1  °I.O  +  °L  I  >  >  +  mL2^L  I  lcL2  C0Sei.: 


+  n^O*eL2(*"®LOsin,0I.O  +  0l 

,  +0,  ,)  +  COS0,  ()cos  (0,  0  +0L1  +0L;)  )  ) 

+  0f1(O.5aL2  +  mL2^1+mL2rL2)  +  '"tAl*cl.2COS,,1.2> 

+  O.50“2(IL2  +  niL2tc2L2) 

+  0o0LI(IL2  +  mL2t2|+.n|2r,2  +  2m( 

A.*cL2COS°f.2 

+  mL2^O<LI(s'l,0LOsin,0U.+°.  1 

)  +COS0IOCOS(0U)  +  0L1)) 

n^L2^L0^cL2  ^  S*I,0LOSil1^  0l.‘i  ^  01 

+,,L2<  +cos0Iocos{«Lo+0L|+0L2))  ) 

+  0Q0L2  ttL2  +  ,nL2^cL2  +  mL2^L  /cL2COS0I.2 

+  mL2VcL2(smeLOsin(0I.O  +  "l 

,  +0,  ,)  +COS0,  f)cos(0LO+0L1  +  0L,))  ) 
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'^^L1^L2^L2^*  IT1L2^cL2  1,1 1  2^1  l^c!  2ct>s0t  2  ^ 

The  development  for  the  right  manipulator  kinetic  energy  parallels  that  for  the  left 
manipulator.  For  the  upper  arm  portion  (Link  RI ),  the  angular  rate  is 

<..R1  =  i)„  +  oR|  (26) 

The  position  vector  is  constructed  by  finding  the  coordinates  of  the  right  shoulder 
and  adding  the  vector  from  the  shoulder  to  the  center  of  mass. 


rR1  =  *ROCOSHRO*0  +  *ROS,n,)RO>''0  +  *cR  I  (27) 

The  time  rate  of  change  of  the  position  vector  is 

'r,  =ZR0W0COsWR0>0-/R./,>0S'n0R0S«+fcRl‘’,Rl>R1  (28) 

After  calculating  the  kinetic  energy  for  Link  Rl  and  grouping  terms  with  common 
angular  rates,  the  resulting  expression  is 

1R,  =  fl0(0.5(lR1  +mR1tcR,  +mR|^0)  (29) 

+  mR.VcRl(sineROSin<0RO  +  0RI)  +COSftROCOS(HRO  +  0R.))  > 

+  0-59ri  (IR1  +mR/c2R1) 

+  Mr|  <fR1  +mRl^cRl  +  ,nRI*R(/cRI  <!ii"0ROsin(BRO+l,Rl>  +  CosBR0COS  ,0RO  +  °R  I »  > 

Angular  rate  of  the  right  forearm  is 


WR2  =  0<>  +  0RI  +  BR2 


(30) 


Its  position  vector  is 


fR2  ^0COSeRO*0  +  *R0s'nBRoyo  +  *R|\|  +  *cR2*r2  (^) 

Differentiating  Eq.  (31)  produces  the  velocity  vector  for  Link  R2  center  of  mass. 

rR2  =  ^RO<nOCOS0ROyo-^Ro<OOS'n®RO^O +^RiwR)yR1  +  *cR20>R2yR1  (^2) 


14 


The  kinetic  energy  resulting  from  substituting  Eqs.  (30)  and  (32)  into  Eq.  (13)  and 
collecting  common  angular  rate  terms  is 

•  r2  =  l»r»,o-5(iR2  +  mRj<;R2  +  mR,^l  +'11^-,,)  (33) 

+  n,R2  WrI  ‘  S,neROSin  ‘  V.  +  °R  I  >  +  COS%COS  «  °R«.  +  '>R  .  >  >  +  mR2*R  1 
+  mR2  VcRZ  <  si,,0Rusin  1  »R«  +  °R  1  +  "r2>  +  tos,»R„cos  '  "ru  +  °R1  +  "r:1  >  1 

+  HrI  (0.5  (IR2  +  inR2^R1  +lnR2^cR2*  +  n,R2*Rlf;R2COS0R2* 

+  ().50r2  (  IR2  +  inR2^cR2* 

+  90t)R|  <IR->  +  mR2*Rl  +  mR2^R2  +  2mR2*RI*cR2COS0R2 

+  mR2*R0*R  I  <  SmOR0  ®M  (  "ro  +  °R  1 1  +  ‘•™(>r„‘-™  <  +  <>R  |  >> 

+  mR2  VcR2  (  sin0ROsin  < °RO  +  °R  I  +  HR2>  +  COS°R0COS  ( °R0  +  °R I  +  ^R:1  >  > 

+  no0R2  UR2  +  mR2*cR2  +  mR2*RI*cR2COS<*R2 

+  mR2/RO^cR2  (  si"0ROsi"  <«RO  +  °R  I  +  °R2>  +  COS°ROCOS  ‘  °R0  +  0R1  +  °R2)  >  > 

+  0RI0R2  0R2  +mR2^cR2  +,nR2^RI^R2COSt)R2^ 

Expressions  for  the  payload  are  considerably  simpler  because  inertial  coordinates 
are  available.  The  angular  rate  is 

«>,,  =  <>,.  (34) 

It  is  not  necessary  to  describe  the  payload  center  of  mass  by  passing  through  either 
shoulder  as  was  the  case  with  the  manipulator  links.  The  position  vector  is 


fp  -  Xpfls  +  Ypfly 


The  velocity  vector  is  also  simpler  because  of  the  inertial  frame. 


r"  =  X,,t^  +  YPtfv 


(35) 


(36) 
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The  payload  kinetic  energy  is  derived  from  substituting  Eqs.  (14)  and  (16)  into  Eq. 


ip  =  ‘  (i,>(t|.  +  M1,. i Xp  +  Yto)  (37) 

After  substituting  the  expressions  for  kinetic  energy  from  Eqs.  (17),  (21),  (25), 
(29),  (33)  and  (37)  into  Eq.  (12)  and  expressing  the  result  in  the  matrix  form  of  Eq.  (3),  the 
inertia  matrix,  M,  is  given  by  Eq.  (38).  Because  the  generalized  coordinates  for  the  pay- 
load  are  referenced  to  an  inertial  coordinate  frame,  the  inertia  matrix  is  decoupled  between 
the  payload  and  the  rest  of  the  system  Coupling  does  exist  between  the  spacecraft  center- 
body  and  each  of  the  manipulators. 


Mn 

M 12 

M 13 

M,4 

M|5 

0 

0 

m2, 

M22 

M23 

0 

0 

0 

0 

M31 

M32 

M33 

0 

0 

0 

0 

M4. 

0 

0 

M44 

M45 

0 

0 

m51 

0 

0 

M54 

M55 

0 

0 

0 

0 

0 

0 

0 

1P 

0 

0 

0 

0 

0 

0 

0 

m, 

0 

0 

0 

0 

0 

0 

0 

Expressions  for  the  individual  elements  in  the  inertia  matrix  are  given  by 


M55  ~  IR2  +  mR2*CR2 


M45  ~  M54  -  M55 +mR2^Rl^CR2COSeR2 


-  M45  +  mR2^Rc/cR2cos  '  +  ®»">) 


R1  VR2' 


M44  ~  M45  +  rRl  +  mR2^Rl^CR2COS®R2  +  mRl^CRl  +  mR2^Rl 
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M14  ~  M4I  -  M44 +^RO^mRl^CRI  +  mR2*R  1  ^  COS0R 1 


(43) 


+  mR2^R0^CR2COS  *0R  I  +  0R2^ 

M33  =  IL2  +  mL2/cL2  (44) 

M23  ~  M32  =  M33  +  mL2*Ll*CL2COS0L2  ^45^ 

M 1 3  =  M31  =  M23  +  mi  2*1.(/CL2COS*0U  +0L2^  (46) 

M22  =  M23  +  ILl+mL2ZLl/cL2COSeL2  +  mLl^CL!+mL2^1  (47) 

M12  =  M21  =  M22  Ao(miAl.l  +miAl)COS0U  (48) 

+  mL2WCL2COs(0Ll+0L2) 

Mn  =  I()  +  M22  +  M44  +  m()*c(2,  +  (m,  ,  +  m,2)r,0  +  (mR,  +mR2)*2RO  (49) 

+  Ao^mRl^CRl  +mR2^Rp  COS0R1  +2mR2^RO^CR2COS^0Rl  +  0R2^ 

+  2^10  (mL  1^CL  1  +mL2^Ll^COS0l.  1  +  2mlA.(/CL2COS  ^0LI  +  0L2^ 

2.  Centripetal  and  Coriolis  Matrix,  G 

The  G  matrix  contains  all  of  the  centripetal  and  Coriolis  terms.  It  is  most  easily 
found  using 


G(q,  q)  = 


gTC<"q 

qTC‘2>q 


lqTC<8>g 

where  the  elements  of  are  defined  by  the  Christoffel  symbol 


r(i)  _ 

jk  - 


\  aqk  aqj  J 


(50) 


(51) 
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The  form  of  the  G  matrix  for  the  system  of  Figure  1  is  given  as 


o,  =  -<lo(»2li  +200Oli)  (m,  ,&u  +■«!,/,,)  Sind,  , 
_inL2/Ll^°U2^L2  (200  +  20i.i  +H|.;)  sinO,  , 


-i,.l^lo/c12(20,)(Ol1  +0,  :)  +  ((),  ,  +»,  ,)*)  sin  <«u  +  »,.,> 

~*ro^0R<  +20„0R|)  (mR]fcR1  +  mR2^R| )  s'n0Ri 
_mR2*Ri/cR20R2  <20o  +  20ri  +  «>R2>  S'«(,R2 

~mR2*RO*CR2(2(V0Rl  +0R2)  +  <0RI  +  »R:)  ")  sin  ( 0R ,  +  0R,) 

G,  =  ^L0(jo(mL/CLl  +l,1L2*ll)  sin0u  “  mi.2*l.l*C|  ;**|.2<20n  +  2«|.,  +'*!.:)  s'*»0L: 

+  mL2WCL2®oSin(0u+0u2) 

2  . 2 

°3  =  mL2^LI/cL2((*Ll +®L2)*s'n0L2  +  ml.:WCI.2,^sin,HU  +  0I.2) 

°4  =  *RO0O  (  mR  |  |  +  ,nR2^R  I )  sin8R  I  -  mR2*R  !  fcR20R2  ( 20O  +  20R  I  +  0R2>  S‘n0R2 

+  mR2fRO&R20OS'n<eR)+0R2> 

G5  =  mR2^Rl^CR2  ^0R'  +0R2)  S'n0R2  +  mR2*R</CR20OS'n  ( 0R1  +  0R2* 


(52) 

(53) 

(54) 

(55) 

(56) 

(57) 
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3.  Generalized  Forces,  Q 

The  generalized  forces  are  found  using  the  principle  of  virtual  work.  When  there 
is  no  reaction  wheel  on  the  centerbody,  the  system  does  not  experience  any  external  forces 
capable  of  performing  work.  Six  joint  actuators  apply  torques  at  the  shoulder,  elbow,  and 
wrist  of  each  manipulator. 

T 

U6  =  ULS  uli-:  ui.w  urs  urh  urwi  ^ 

u^,  is  simply  the  joint  actuator  subset  of  the  complete  actuator  torque  vector,  u.  The 
total  virtual  work  is  the  sum  of  the  torques  applied  to  the  individual  bodies  times  their  vir¬ 
tual  angular  displacements. 

N  M 

5W  =  £  5Wt  =  ]T  (Mi)50i  (59) 

i  =  I  i  =  I 

When  the  left  shoulder  joint  actuator  applies  a  positive  torque  on  Link  LI,  a  nega¬ 
tive  torque  is  also  appiied  to  the  centerbody.  The  virtual  work  performed  by  the  left  shoul¬ 
der  motor  is 

5WLS  =  ULS<50o  +  60Ll-5e())  <60> 

where  the  positive  angles  are  those  associated  with  the  change  in  Link  LI  attitude  and  the 
negative  angle  is  associated  with  the  change  in  centerbody  attitude.  The  left  elbow  actua¬ 
tor  makes  a  positive  contribution  to  Link  L2  attitude  and  a  negative  contribution  to  Link 
LI  attitude. 

5Wle  =  ULE (50o  +  50L1  +50j  t -50()-50]  j)  =  U]  e50L2  (61) 

The  joint  actuator  at  the  left  wrist  makes  a  positive  change  in  the  payload  attitude 
and  a  negative  change  in  Link  L2. 

5WLW  =“LW<5V50O-6SL,-5eL2>  <62> 
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The  right  shoulder  actuator  makes  a  positive  contribution  to  Link  R1  attitude  and  a 
negative  contribution  to  centerbody  attitude 

=  urs^0o  +  ^0ri  =  urs^®ri 

Link  R2  has  a  positive  virtual  displacement  due  to  a  positive  torque  at  the  right 
elbow  The  same  torque  causes  a  negative  virtual  displacement  of  Link  Rl. 

^WRE  =  URE^0()  +  ^0R1  +  ^0R2  -80O-80R1)  =  URE^®R2 
The  right  wrist  actuator  has  a  positive  influence  on  the  payload  and  a  negative 
influence  on  Link  R2. 

SWRW  =  URW  (50P  ~  50o  “  S0R,  -  80R2)  (65) 

Gathering  Eqs.  (60)-(65)  together  produces 

5W  =  (-  ULW  “  URW^®<)  +  ^UI.S  ~  UI,W^0L1  +  ^UEE  ”  ULW^0L2 

+  (urs~urw)50ri  +  (URE  ~  URW^®R2  +  ^U].W~  W§0P 
With  respect  to  the  system  equations  of  motion,  the  generalized  force  correspond¬ 
ing  to  a  particular  generalized  coordinate  is  that  portion  of  the  virtual  work  associated  with 
the  same  generalized  coordinate.  Now  Eq.  (66)  can  be  transformed  into  a  matrix  form. 

96  =  BA,  <67> 

where  B  is  the  control  influence  matrix  given  by 


0  0  -10  0  -1 
10-100  0 
0  1-10  0  0 

0  0  0  10-1 
0  0  0  0  1  -1 

00  100  1 
0  0  0  0  0  0 

0  0  0  0  0  0 


(68) 
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The  only  effect  of  a  positive  reaction  wheel  torque  applied  to  the  spacecraft  is  to 
alter  the  centerbody  attitude  in  a  positive  direction  This  manifests  itself  in  the  control 
influence  matrix  in  the  form  of  another  column  This  new  column  is  all  zeros  except  for  a 
single  one  corresponding  to  the  location  of  the  reaction  wheel  torque  in  the  u  vector  With 
the  reaction  wheel  torque  as  the  first  element  in  the  control  vector  as  in  Eq  (2),  the  com¬ 
plete  control  influence  matrix  is 


I  0  O  -  I  0  0  - 1 
0  10-1000 
0  0  1-10  0  0 
0  0  0  0  I  0-1 
0  0  0  0  0  1-1 
000  1  00  1 
0  0  0  0  0  0  0 i 
0  0  0  0  0  0  0 


(69) 


4.  Constraints  Matrix,  A 

Because  the  eighth  order  system  under  consideration  has  only  four  degrees  of  free¬ 
dom,  an  additional  four  equations  are  needed  to  describe  the  constraints.  The  eight  gener¬ 
alized  coordinates  are  not  independent.  The  constraint  equations  embody  the  information 
that  the  manipulators  are  both  grasping  the  payload  forming  a  closed  chain  system.  The 
constraints  matrix  is  derived  by  writing  the  system  constraints  in  the  Pfaffian  form  as 

Aq  +  A()  =  0  (70) 

These  equations  come  from  geometric  relationships  of  expressing  the  payload  cen¬ 
ter  of  mass  Cartesian  coordinates  in  terms  of  the  other  generalized  coordinates. 


tLHcos  (0() +0LO)  +rL1cos(0o  +  0LO  +  Ou)  ,cos(n„+«|0+«L1  +0,  ,)  +  tcpCOS0p  =  Xp 
AOsin<e0  +  eL0)  +*LIsin<0o  +  OLO  +  0LI)  +  A:*'"  +  0u.  +  Hi .1  +  hi.j>  +^psin0p  =  yp 

Vocos  <e0  +  0RO)  +  ^ ,  cos  ( 0O  +  eR0  +  0R , )  +  *R2coS  ( 0„  +  0RO  +  0R ,  +  »R2>  -  Up  -  icp)  COS0p  =  Xp 

VoS'n  +^R0^  +  Vl  S‘n  "V  +  *Vo  +  *V|^  +  V:S’M  "V  +  'Vo  +'Vl  +  'V’*  ~  '  V  _  s'n®p  =  .  p 


(71) 
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To  get  the  PfafFian  form  of  Eq.  (70),  differentiate  Eq  (71)  and  rearrange  terms 
The  result  is 


i«ul 


An 

Ai: 

A13 

() 

0 

A,-. 

-1 

0  1 

(I. 

A2t 

A22 

A23 

0 

0 

A2, 

0 

-r 

«>K. 

A3, 

0 

0 

A  34 

A 

A,„ 

-1 

0 

" 

•T 

< 

0 

0 

A44 

A4„ 

0 

-li 

Op 

iol 

xP 

Y,> 


The  constant  term,  A(),  is  a  zero  vector.  The  individual  element  in  the  constraints  matrix 


are  given  by  the  following  equations 

Ai6  =  -*ci>sin0p  (73> 

A26  =  ^cpcos0p  (74> 

A36  =  (^,-^Cp)  sin0p  (75) 

A46  =  ~  (V^Cp)  COS0P  (76> 

A45  =  ^R2COS^0  +  ®R0  +  ®Rl  +®R2^  (77) 

A44  =  A45+^R1COS^()  +  ®RO+®R1^  (78) 

A41  =  A44  +  *K0COS  + 

A3S  =  -^R2sin(0o  +  0Ro  +  0Rl +0R2^  (80) 

A34  =  A35-^Rlsin^O  +  ®RO  +  eRl^  ^81) 

A31  =  A34_^ROS‘n^O  +  ®RO^  ^82^ 

A23  =  ^L2COS  +  0LO  +  ®LI  +®L2^ 
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A22  "  A23  +*UCOS^()  +  ®L0  +  ®I.  [) 

(84) 

A21  =  A22  +  ^|()cos(0o+0Lo) 

(85) 

S3  =  -^L2sin  (0O  +  0J  0  +  0,  J  +  0!  :) 

(86) 

A12  =  A13-Alsin(0n  +  0l.o  +  ei.I> 

(87) 

All  ~  A12~^I.0S*n  ^0(>  +  0LO^ 

(88) 

If  the  manipulators  are  mounted  on  a  fixed  platform  rather  than  a  rotating  base,  an 
additional  constraint  equation  is  included  in  the  A  matrix.  The  constraint  is  that  0q  is  con¬ 
stant  and  therefore 

<>„  =  o  (89) 

This  constraint  is  augmented  into  the  A  matrix  by  adding  a  fifth  row.  The  first  ele¬ 
ment  in  the  row  is  a  one.  The  remaining  seven  elements  are  all  zeros 

C.  SIMPLIFIED  EQUATIONS  OF  MOTION 

The  potential  energy  term  is  zero  because  motion  is  confined  to  the  horizontal  plane 
and  the  system  is  composed  of  rigid  members.  The  inertia  matrix,  G  matrix,  B  matrix,  and 
constraints  matrix  can  be  found  from  the  results  of  the  previous  sections.  The  remaining 
unknowns  are  the  actuator  torques  and  the  Lagrange  multipliers  By  using  the  equations 
of  motion  and  the  Pfaffian  form  of  the  constraints,  one  can  eliminate  the  Lagrange  multi¬ 
pliers.  The  time  derivative  of  Eq.  (70)  is 

Aq  +  Aq  =  0  (90) 

Solving  Eq.  (10)  for  q  and  substituting  the  result  into  Eq.  (90)  permits  one  to  find  an 
expression  for  the  Lagrange  multipliers. 
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- 1  T  _  I  _  | 

X  =  (AM  A  )  (AM  ‘(G-Bu) -An)  (91) 

The  inertia  matrix  is  always  a  square  matrix  with  full  rank  and  therefore  invertible  To 
investigate  the  invertiblity  of  AM  1 A  1 ,  begin  by  creating  a  4x4  matrix  out  of  the  third, 
fifth,  seventh,  and  eighth  columns  of  the  constraints  matrix 


'  A  n  0-10 


;  A23  0 

j  0  A  35 
I  0  A45 


0  -1 
-1  Oj 


(92) 


Inspection  of  this  submatrix  reveals  that  ail  of  the  rows  and  columns  are  linearly  inde¬ 
pendent  even  if  A 1 3  =  A23  and  A35  =  A45.  Therefore,  the  A  matrix  always  has  rank  of  4 
The  4x4  matrix  product  AM  A  will  also  always  have  rank  of  4  and  is  therefore  invert¬ 
ible.  Eq.  (91)  can  be  substituted  back  into  the  equations  of  motion  (Eq.  (10))  leaving  the 
actuator  torques  as  the  only  unknowns  The  resulting  equations  of  motion  in  which  the 
Lagrange  multipliers  have  been  removed  and  potential  energy  is  zero  are 


where 


Mq  + G  =  Bu 


(93) 


,-l  *  I 


G  =  G  -  A  (AM  A  )  (AM  G-Aq) 


f  _  j  \ 

B  =  ( I  -  A 1  (AM- 1  A  F)  AM- 1 J  B 


(94) 

(95) 


D.  REFERENCE  TORQUES 

Given  a  reference  trajectory  of  the  payload  with  known  displacements,  velocities  and 
accelerations,  one  can  use  the  simplified  equations  of  motion  (Eq.  (93))  to  solve  for  the 
actuator  torques  that  will  produce  the  reference  trajectory. 
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(96) 


M  q  .  +  G  -  B  ,u 
vcl  *rel  lt-1  "-1  ret 

The  equations  tor  specific  elements  in  the  matnees  of  Eq  (96)  are  the  same  as  already 
presented  The  subscript  “ref’  merely  means  that  the  displacement,  velocity  and  accelera¬ 
tion  terms  are  the  values  from  the  reference  trajectory 

In  this  study,  the  total  number  of  actuators  is  more  than  the  system  degrees  of  freedom 
This  situation  is  caused  by  the  geometric  constraints  of  multiple  manipulators  handling  a 
common  object.  As  a  result,  there  are  an  infinity  of  solutions  for  the  reference  torques 
One  method  to  select  a  SDecific  solution  is  to  establish  a  cost  function.  An  obvious  cost 
function  is  *o  minimize  a  weighted  norm  of  the  actuator  torques 


J  =  u 1  W  u 


(97) 


2  -  ret  11  ret 

The  problem  now  becomes  one  of  minimizing  the  cost  function  (Eq,  (97))  subject  to 
the  constraint  that  the  reference  equations  of  motion  (Eq.  (96))  are  satisfied  Augmenting 
the  cost  function  with  the  constraint  by  means  of  another  Lagrange  multiplier  leads  to 


J  =  -  uT  ,W  u  r  +  y*  (Br„,-u  -M  ,-q  .  -  CL.,-1  (98) 

2  ret  u-ref  -  V  rel  rel  rePrel  rclS  v  ' 

The  minimum  of  the  augmented  cost  function  is  found  by  taking  the  gradient  of  Eq. 

(98)  with  respect  to  the  reference  torques  and  with  respect  to  the  Lagrange  multiplier. 

Each  of  the  gradients  is  set  to  zero. 


V*J  = 0  =  w.Vr+Cr 

(99) 

vrJ  =  0  =  Bre(u  -M(ef§  -Gref 

(100) 

Eqs.  (99)  and  (100)  are  two  equations  in  two  unknowns  (y,  ttref)-  Eliminating  y 
results  in  an  expression  for  the  reference  actuator  torques. 
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,-lA 


u„r  =  wu  BrcABrefWu  B,  J  i-  Grcl-j  (101) 

Although  the  matrix  product  nrc,-wi(lnir,  is  an  8x8  matrix,  it  is  not  invertible  A 
pseudo-inverse  is  needed  because  the  system  has  only  four  degrees  of  freedom  There¬ 
fore.  the  matrix  product  BrefwulHtef  is  rank  deficient  and  has  a  rank  of  four  at  most  This 
expression  tor  reference  actuator  torques  minimizes  the  augmented  cost  function  (Eq 
(98))  at  each  instant  in  time.  Although  the  value  for  the  reaction  wheel  torque  is  calcu¬ 
lated.  it  is  not  minimized  by  this  function  The  reaction  wheel  torque  profile  is  dictated  by 
the  disturbance  torques  transmitted  to  the  centerbody  as  a  result  of  manipulator  and  pay- 
load  motion  For  a  given  reference  trajectory,  an  infinite  variety  of  joint  actuator  torques 
can  produce  that  trajectory  However,  a  given  reference  trajectory  has  only  one  reaction 
wheel  torque  profile  that  is  common  to  all  the  infinity  ofjoint  torque  combinations  associ¬ 
ated  with  that  trajectory.  Equation  (101)  selects  from  among  the  infinity  of  ioint  actuator 
torques  the  one  combination  that  minimizes  the  weighted  norm  cost  function.  Although 
the  selection  is  limited  to  a  single  choice.  Equation  (101)  also  produces  the  correct  reac¬ 
tion  wheel  torque  for  the  given  reference  trajectory 


E.  LYAPUNOV  CONTROLLER 

This  material  in  this  section  is  based  on  Ref  16.  The  purpose  of  any  control  law  is  to 
provide  system  performance  that  satisfies  a  specification.  As  a  bare  minimum,  the  control 
law  must  keep  the  system  stable.  Because  of  the  highly  nonlinear  nature  of  this  spacecraft 
robotics  system,  most  control  laws  simply  do  not  apply.  The  motivation  behind  using 
Lyapunov  methods  is  to  develop  a  control  law  with  guaranteed  stability.  Recall  the  equa¬ 
tions  of  motion  of  the  manipulator  system  are 

Mq  +  G  =  Bu  (102) 
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Solving  Eq.  (102)  for  q  results  in 


q  =  M  1  (Bu-G)  (103) 

Substituting  Eqs.  (94)  and  (95)  back  into  Eq  (103)  and  grouping  terms  according  to 
the  form 

q  =  C1u  +  C2q  +  C1  (104) 

leads  to  the  following  expressions 

C,  =  M_1  {I- AT(AM"‘a1)  'anT'jB  (105) 

C2  =  -M_lAr(AM_lAT)  'A  (106) 

C3  =  M~‘  {AT(AM_1AT)  'aM_1-I)G  (107) 

Similarly,  the  reference  maneuver  accelerations  can  be  expressed  as 


9ref 


-  c,  u  f  +  C,  tj  +C, 
hcrrcf  2^'rcl  3, 


ref 


(108) 


where  again  the  reference  subscripts  on  the  C  matrices  indicate  that  reference  maneuver 
values  need  to  be  used  in  their  calculation.  Let  error  quantities  between  the  actual  vari¬ 
ables  and  their  reference  maneuver  counterparts  be  defined  by 


=  9  -  9ref 

(109) 

59  =  9  “  <itcr 

(110) 

59  =  9  -  9rcf 

(111) 

Now  define  an  error  Lyapunov  function  as 

U  =  0.5  (5q  •  5q)  +f(5q) 

(112) 

where  f(Sq)  >  0.  Differentiating  Eq.  (112)  results  in 
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(113) 


U  =  5q • 5q  + 


I 


P 


Pf 

(Sq,) 


H 


Let 


F  =  _i£ _ 5L- 

[d(5q,)  t)(5q2) 
Then  Eq.  (114)  can  be  rewritten  as 


Of 


T 


<?(Sq7) 


(114) 


0  =  5q(5q  +  F)  (115) 

Substituting  Eq.  (104)  and  Eq.  (108)  into  Eq  (111)  and  then  Eq.  (Ill)  into  Eq.  (115) 
produces 

0  =  8q.  ((C.u-C,  uref)  +  (C2q-C2  q(cf)  +  (C,-C,<()+F)  <11«> 

If  one  lets  the  quantity  inside  the  brackets  of  Eq.  (116)  equal  -Ky8q  where  Kv  is  a 
positive  definite  matrix,  then  one  is  guaranteed  that  U  <  0  and  therefore  the  system  will  be 
stable  in  the  Lyapunov  sense.  Solving  Eq.  (116)  for  command  torques,  u,  leads  to 

u  =  C,t  ,-Kv8q+C|recUri!f-  (C2q-C2i|qrer)  -  (C,-C,J  -F)  (117) 

C]  is  an  8x7  matrix  so  C  ^  is  its  pseudo  inverse.  Equation  (119)  finds  the  torques  that 
should  be  used  rather  than  the  reference  torques.  All  that  remains  is  to  choose  a  function 
for  f  (5q) .  One  can  choose 

f(5q)  =  ^5q  rKp5q  (118) 


where  like  Ky,  Kp  is  required  to  be  positive  definite.  Selection  of  values  for  the  gain 
matrices  is  beyond  the  scope  of  this  work.  The  simulations  included  in  the  next  chapter 
use  diagonal  matrices  with  uniform  values  simply  as  a  matter  of  convenience.  One  might 
try  to  adapt  the  linear  quadratic  regulator  (LQR)  problem  to  find  more  optimal  gains. 
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After  substituting  Eq.  (118)  into  Eq.  (114)  and  that  result  into  Eq.  (119),  one  obtains  the 
final  form  of  the  Lyapunov  controller. 

u  =  C  t  {-Kv5q  +  Cleuref-  (C2q-C2>eqrcf)  -  (C3  -  C3  )  -  Kp5q}  (119) 

If  the  differences  between  the  reference  trajectory  and  the  system  dynamics  are  small, 
the  Lyapunov  controller  approaches  the  form  of  a  proportional  plus  derivative  (PD)  con¬ 
trol  law. 

F.  REFERENCE  TRAJECTORIES 

The  reference  trajectories  describe  the  nominal  path  that  the  system  follows  in  moving 
from  the  initial  conditions  to  the  desired  final  conditions.  One  need  only  specify  reference 
trajectories  for  as  many  generalized  coordinates  as  there  are  degrees  of  freedom.  In  effect, 
the  generalized  coordinates  can  be  divided  into  two  sets.  One  set  contains  the  minimum 
number  of  coordinates  needed  to  completely  describe  the  system.  The  second  set  contains 
all  remaining  coordinates,  (redundant  coordinates).  The  choice  of  which  generalized 
coordinates  to  specify  is  entirely  arbitrary.  A  reasonable  choice  includes  the  payload  coor¬ 
dinates  and  centerbody  attitude  since  the  user  will  probably  be  especially  interested  in 
these  generalized  coordinates.  The  redundant  coordinates  are  the  four  manipulator  joint 
angles.  Given  reference  trajectories  for  the  minimum  number  of  coordinates  exist,  the 
redundant  generalized  coordinates  can  be  derived.  This  research  assumes  trajectories  are 
available  which  define  displacement,  velocity  and  acceleration  for  the  centerbody  attitude, 
payload  attitude,  and  payload  center  of  mass  coordinates  (Xp  Yp  0p  and  0O). 

1.  Calculating  Redundant  Coordinates 

Figure  2  illustrates  the  relevant  geometrical  relationships  to  find  the  joint  angles  of 
the  left  manipulator.  Xp  Yp  0p  and  0q  are  obtained  from  the  reference  trajectory.  Point 
LS  is  the  left  shoulder  joint.  It  has  Cartesian  coordinates  given  by 
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Figure  2:  Deriving  Left  Manipulator  Joint  Angles 


LSx  ■  V=os<,,..+0iV  (12°) 

LSy  =  *L0si,,<  VW  (121) 

Point  Q  is  the  joint  between  the  manipulator  end  and  the  payload.  The  Cartesian 


coordinates  of  this  point  are 


The  distance  between  the  left  shoulder  and  Point  Q  is  given  by 

lsq  =  J(Qx-LSx)2+  IQy-LSy>2  (124) 

The  inertial  angle  formed  by  the  vector  from  LS  to  Q  is 


P(  =  atan 


(125) 


The  dimensions  of  the  triangle  formed  by  the  manipulator  joints  are  known  Using 
the  law  of  cosines,  the  interior  angles  at  the  shoulder  and  elbow  can  be  found  from 


P2  = 


acos 


r<i+isQ2-£] 
V  2t,  ,LSQ  J 


(126) 


acos 


*l,+4-LSQ2 

AA: 


(127) 


All  that  remains  is  to  algebraically  construct  the  manipulator  joint  angles  from 
other  angles  as  follows 

gli  -  Pi+P2-‘0«+9lo)  02*) 

0L2  =  p,  + 180'  (129) 

The  development  for  the  right  manipulator  is  similar.  Its  geometry  is  depicted  in 
Figure  3. 

Point  RS  is  the  right  shoulder  joint  with  Cartesian  coordinate 

Rsx  =  *rocoS(00  +  Oro)  (130) 

\ 

RSy  =<R0sin(«,l  +  0RO)  (131) 

Point  P  is  the  joint  between  the  manipulator  end  and  the  payload.  The  Cartesian 
coordinates  of  this  point  are 

p*  =  xP+(V<cp)c°sep  (I32) 

py  =  Yp+(tp-tc|>)sin0p  (133) 

The  distance  between  the  right  shoulder  and  Point  P  is  given  by 
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Figure  3:  Deriving  Right  Manipulator  Joint  Angles 


From  the  law  of  cosines,  the  interior  angles  at  the  shoulder  and  elbow 


fL  =  acos 


^ri  +  *R2  rsp 

2W*2 


(137) 


The  geometry  in  Figure  3  gives  the  manipulator  joint  angles  based  on  the  other 


angles 

Or  I  »  03«) 

9R2=18«»-p6  (139) 

Recall  from  the  discussion  of  the  Lyapunov  controller  that  torques  are  calculated 
based  not  only  on  the  generalized  coordinates  but  their  velocities  and  accelerations  as 
well.  The  redundant  coordinates  have  just  been  found,  but  the  redundant  coordinates 
velocities  and  accelerations  must  still  be  developed. 

Differentiating  Eqs.  (122)  and  (123)  expresses  the  velocity  of  Point  Q. 


Q*  =  Xp  +  hp^pSinOp  (140) 

Qy  =  Yp-Op^pCosGp  (141) 

But  the  coordinates  of  Point  Q  can  also  be  expressed  in  terms  of  left  manipulator 
variables. 

Q*  =  *LOCOS<0O+eLO>  +/L1COS(0O+0LO+°U)  +  *L2C0S  ,B0  +  %  +  9U  +  0L2>  (142) 

Qy  *  *LOS'n(eo+0LO>  +*Llsin(0O  +  OI.O  +  °U)  +  *I.2S'n  (0O  +  0I.O  +  0U  +  °L2>  (143) 

Differentiate  these  equations  and  rearrange  the  terms  in  the  form  of 


Q* 

where 

Dj  (  1,  l)  =  ~^L2S'"  *0O  +  0LO  +  0|.l  +  "l.2*  ~  *L  lS'n  <0O  +  0LO  +  0L|! 

D2d,2)  =  -^2sin(9o+eLO  +  0L|+OL2) 

°2  (2.  1)  =  ^L2C0S  <0O  +  0LO  +  0I.  I  +  01.2*  +^I.IC0S  ,0O  +  0LO  +  0LI> 


=  D,0o  +  I>, 


'L2 


(144) 
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(145) 

(146) 

(147) 


D2<2,2)  =  t^cos  <<*„  +<•,  i,  +  (,L|  +(V:) 

15,  (1,1)  =  -/L2sin  (eo  +  0LO  +  0L1  +*1L2)  -  /,  ,siiH0()+0l  n  +0L|)  -e)  ()sw  <0o  +  Blo) 

1)|  (  1,  2)  =  *L2eos  (90  +0LQ  +  0!  |  +  0L2)  +  t j  !  cos  <9,,  +G(  (( +  0(  j  |  + „cos  <  9q  +0L()I 

Left  manipulator  joint  velocities  are  found  by  rearranging  Equation  (144). 


(14*) 

(149) 

(150) 


0L  1 

=  Dj' 

Q, 

i 

-o,rt0 

»L2 

J 

(151) 

where  Eqs.  (140)  and  (141)  provide  the  expressions  for  q,  and  Qy  respectively. 

Using  the  same  approach  to  find  the  joint  velocities  of  the  right  manipulator.  Point 
P  is  expressed  as  a  function  of  right  manipulator  variables 

Px  =  *R0COS  <0O  +  ®RO^  +*RIC0S  <0O  +  <>R«  +  0RI^  +*R2COS  <0n  +  0RO  +  °R  I  +  0R2*  (152) 

Py  =  ^R0S'n  +  HRol  +*Rlsin<0O  +  °R»  +  (lRI)  +  *R2S'n  * 0O  +  0RO  +  0R1  +  0R2^  (153) 

Differentiate  these  equations  and  rearrange  the  terms  in  the  form  of 


=  iV»o  +  n>4 


’Rl 


(,R2 


(154) 


where 


D,(l.  1)  =  ~^R2S‘n  *0O  +  0RO  +  0RI  +  0R2*  ~^RlS'n(0O  +  0RO  +  0RI^ 

D4(l,2)  =  -^R2S'n  (0O  +  0RO  +  0RI  +  0R2* 

D4(2, 1)  =  tR2cos(9o  +  0RO  +  9R|  +  9R,)  +  tR1cos(90  +  9R0  +  9RI) 

D4(2,  2)  =  rR2cos(90  +  0R0+«R1 +9R2) 

Dj  ( 1, 1)  =  -tjjjsin  (0O  +  9ro  +  0rj  +0R2)  ~ *ri  s'n  (0o  +  0ro  +  0ri>  "  *Rosin  ^0o  +  0ro^ 

Dj  (1,2)  =  *R2cos<0o  +  0ro  +  0ri  +9R2)  +  tR]  cos  (0() +  0RO  + 0R, )  +<R0cos  (0O  +  0RO) 

Right  manipulator  joint  velocities  are  found  by  rearranging  Equation  (154) 


(155) 

(156) 

(157) 

(158) 

(159) 

(160) 
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K1  =  i) 1 

i  ‘-'a 

,>.  j 

Kj 

i\  s 

(161) 


where  expressions  for  Fx  and  pv  are  found  by  differentiating  Eqs  ( 132)  and  (133). 


=  X,,-!),,  -  t  ,,»  sintlp 

(162) 

=  Y,, +  costtp 

(163) 

Manipulator  joint  accelerations  are  found  by  differentiating  the  expressions  for 
velocity  (Eqs.  ( 144)  and  (154)). 


Q, 

•  •  0.  ,1  0,  , 
=  D,H0  +  D  0(1  +  I):I  1  +(),; 

(164) 

Qy 

I''.: 

«'»1.2 

K 

=  d3o0+d,ou  +  i)4(0ri 

"ri 
+  l)4,  R1 

(165) 

h 

('r2 

"r2 

Solving  for  joint  acceleration  gives 


- 

f 

r  i 

f. 

=  D2' 

•o 

-  !  o, , 

V 

[Q*J 

L«L2 

> 

®RI 

«  °4 

/ 

K 

r  ~ 

-  •  !<>R| 

\ 

V 

h 

l_f*R2 

/ 

(166) 


(167) 


where  the  accelerations  of  Points  Q  and  P  come  from  differentiating  Eqs.  ( I40)-(  14 1 )  and 
Eqs.  ( 1 62)-(  1 63).  Derivatives  of  the  D  matrices  are  constructed  by  differentiating  Eqs. 
( 1 45)-(  1 50)  and  Eqs.  ( 1 55)-(  1 60). 


2.  Selecting  Reference  Trajectories 

Any  path  which  connects  the  associated  endpoints  can  be  a  reference  trajectory. 
To  help  ensure  that  the  spacecraft  and  payload  do  not  experience  any  unnecessary  jerk  or 
excitation  of  flexible  structures,  one  might  further  constrain  the  path  such  that  the  veloci- 
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ties  and  accelerations  are  zero  at  the  endpoints  Because  a  reaction  wheel  is  required  to 
maintain  spacecraft  attitude,  the  reaction  wheel  torque  history  is  a  prime  candidate  for 
optimization  Possible  performance  indices  include  the  integral  of  the  absolute  value  of 
reaction  wheel  torque 

h 

1  =  /|‘Wi,dl  <16»> 

t 

o 

or  the  maximum  reaction  wheel  torque. 

J  =  max  (|uvvheejj)  (169) 

A  rigorous  method  for  reference  trajectory  selection  is  to  develop  an  optimal  con¬ 
trol  solution  to  the  two  point  boundary  value  problem.  The  performance  index  in  the  opti¬ 
mal  control  problem  is  given  by 

i  =  JL  [x  ( l) ,  m  n .  tj (170) 
Using  Eq.  (168)  as  an  example, 

L  =  |uj  =  iDul  (171) 

where 


and 


LS 


LE 


ULVV  URS  URE  “rwIO 


(172) 


D  =  [i  o  o  o  o  o  o]  (173) 

The  state  equations  must  be  formulated  as  first  order  differential  equations  as 

x  =  f[x(t),u<t),t]  (174) 

Because  the  system  dynamics  of  my  problem  are  second  order  differential  equa¬ 
tions,  the  state  vector  for  the  trajectory  optimization  is  a  combination  of  generalized  dis¬ 
placements  and  velocities. 
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(175) 


The  resulting  state  equations  are 

(176) 

where  o  and  B  are  the  same  matrices  as  already  found  in  Eqs  (94)  and  (95)  respectively. 

Desirable  bounoary  conditions  are  such  that  the  payload  is  at  rest  with  zero  accel¬ 
eration  at  the  beginning  and  end  of  the  repositioning  maneuver  However  because  the 
state  vector  does  not  contain  accelerations,  they  cannot  be  specified  as  a  boundary  condi¬ 
tions.  If  the  state  vector  is  increased  to  include  accelerations,  then  the  first  order  state 
equations  involve  third  order  derivatives  of  the  equations  of  motion  rather  than  second 
order  equations.  This  prevents  including  payload  accelerations  as  part  of  the  boundary 
conditions.  To  permit  further  development  of  the  optimal  control  problem,  the  boundary 
conditions  will  be  limited  to  desired  positions  and  zero  velocity 


*“u>  =  h".,>  °,J 

(177) 

X(tf)  =  Lq".)  °.*8jT 

(178) 

The  Hamiltonian  formed  by  combining  the  performance  index  with  the  state  equa¬ 
tions  is 

H[x(t),u(t),X(t),t]  =  L[x(»),ufl),i]  +xT(,)l'[x(t),u(l).t|  (179) 

The  performance  index  and  the  state  equations  are  both  linear  with  respect  to  the 
control  vector,  u.  The  consequences  of  this  are  that  one  cannot  find  a  minimum  by  taking 
the  gradient  with  respect  to  u  and  setting  it  equal  to  zero.  The  applicable  control  form  is 
bang-bang.  Separating  the  Hamiltonian  into  those  terms  which  premultiply  u  and  those 
which  do  not  leads  to  the  control  law 


I  n 

h  = 

; 


i  0  1 

8x8  '8x81, 


Vrl  V, 

X  +  i  II  +  | 

(Vx8  "sxK!  M  *nj  i-M  V. 
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where 


u  =  W^1"11,' 


H  =  li., +  11,11 


The  other  equations  which  must  be  satisfied  are 


+>n  _  /'i.  ,i  or 

Ox  Ox  Ox 


(180) 


(181) 


(182) 


Because  the  performance  index  is  only  a  function  of  u,  the  first  portion  of  the 
above  necessary  condition  is  trivial. 

OL 


Ox  1x16 


(183) 


—  is  not  as  easily  found.  The  M,  g,  a,  and  a  matrices  are  all  functions  of  the  state 

Ox 

vector.  In  addition,  the  complexity  is  increased  by  several  matrix  inversions  in  the  expres¬ 
sion  of  r  in  the  6  and  b  matrices.  Although  an  analytical  expression  may  be  theoretically 
possible,  finding  it  was  found  to  be  extraordinarily  tedious. 

Recall,  however,  that  the  usefulness  of  the  reference  trajectories  is  to  specify  the 
generalized  coordinates,  velocities,  and  accelerations.  Therefore,  a  convenient  form  for 
the  reference  trajectory  is  as  a  polynomial  function  of  time.  The  following  development 
uses  the  payload  attitude  generalized  coordinate  to  illustrate  how  the  polynomial  reference 
trajectories  are  applied.  Let 

A0p  =  9p(tf)  -0p(to)  (184) 

where  is  the  maneuver  start  time  and  tf  is  the  final  time.  The  duration  of  the  maneuver 
is  the  difference  between  tf  and  tg.  0p(to)  and  0p(tf)  are  the  initial  payload  attitude  and  the 
desired  final  attitude  respectively.  If  the  desired  reference  path  for  the  payload  attitude  in 
moving  from  initial  to  final  conditions  is  a  curve  which  can  be  represented  as  a  polyno¬ 
mial  function,  f(x),  where  x  is  simply  normalized  time 
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then 


1 » 

T  = 

(185) 

0p  (t)  =  0  ( l,,J  Hltll  \0..| 

ffl 

(186) 

0P  (t)  =  fit)  (AO  )  1  '  - 

(187) 

(  I  ) 

oP  (t)  =  r  < x)  i  \o.,(  — 

(188) 

In  order  for  Eq.  (186)  to  produce  the  correct  initial  and  final  values  for  e  ,  the 

r**< 

polynomial  must  be  such  that 

f(x=0)  =  0  (189) 

f(x=l)  =  I  (190) 

To  produce  zero  velocity  and  acceleration  at  the  initial  and  final  conditions 
requires  that  f(x)  also  satisfy 

f  (x=0)  =  0  (191) 

f  (x=  1 )  =  0  (192) 

f(x=0)  =  0  (193) 

fi'(x=l)  =  0  (194) 

The  minimum  order  polynomial  which  satisfies  the  boundary  conditions  of  Eqs. 
(189)-(194)  is 


f  (x)  =  6x5  -  15x4+  10x3 


(195) 


The  expressions  for  payload  reference  trajectory  using  the  fifth  order  polynomial 


become 


0P  f(t)  =  0p  (tQ)  +  (6t5  -  15x4  +  10x3)  (A0p) 


(196) 
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(197) 


0P  (t)  =  (30x4  -60t3  OOt2)  (A0(>)  !  ' 

rel  1  I  •  -  t 

Vlt  () 

0,,  (t)  =  (120x3  -  180t2  +60t)  (A0„)  f  '  !  (198) 

l(tr->u)2J 

The  polynomial  reference  trajectory  is  also  be  applied  to  the  other  generalized 
coordinates  which  form  the  minimum  set  to  describe  the  svstem  (i  e.  centerbody  attitude 
and  payload  center  of  mass  coordinates).  The  redundant  generalized  coordinates  are  cal¬ 
culated  from  the  reference  coordinates  as  described  earlier 

Higher  order  polynomials  can  increase  the  complexity  of  the  path  but  offer  the 
advantage  that  an  infinity  of  polynomial  coefficients  satisfy  the  position,  velocity,  and 
acceleration  boundary  conditions.  The  selection  of  the  coefficients  affords  an  opportunity 
to  optimize  the  reaction  wheel  torque.  In  this  system,  manipulator  actuator  torques  are 
internal  while  the  reaction  wheel  torque  is  the  only  external  torque.  Therefore,  the  reac¬ 
tion  wheel  torque  will  be  equal  to  the  rate  of  change  of  angular  momentum  which  can  be 
calculated  directly  from  a  reference  trajectory.  This  technique  is  more  computationally 

efficient  because  it  does  not  require  the  construction  of  the  o  and  B  matrices. 

In  general,  the  angular  momentum  about  the  inertial  origin  for  each  member  of  the 
system  is 

H  =  I  (»>  +m  (r  x  v  )  (199) 

-i  >  i  'ii  '  ' 

where  it  is  the  moment  of  inertia  of  the  ith  body  about  its  center  of  mass 

«  is  the  angular  rate  of  the  I'*'  body 
mi  is  the  mass  of  the  i^  body 
r  is  the  inertial  position  of  the  ith  body  center  of  mass 
y  is  the  inertial  velocity  of  the  i1’1  body  center  of  mass 
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The  angular  rate,  position  and  velocity  vectors  were  previously  developed  in  con¬ 
nection  with  determining  kinetic  energy  Those  expressions  require  some  coordinate 
transformations  to  express  all  the  terms  with  respect  to  the  inertial  coordinate  frame.  The 
change  in  angular  momentum  is  found  by  differentiating  Eq  (199)  to  produce 

H  =  I  o>  +m  tr  »  a  )  (200) 

I  1 —  |  1  1  I 

The  total  system  change  in  angular  momentum  is  the  sum  of  change  in  angular 
momentum  for  each  of  the  members.  After  collecting  terms  with  common  angular  veloc¬ 
ity  or  acceleration  terms,  the  expression  for  the  system  change  in  angular  momentum  is 


(201) 


+  2mR21Rl1cR2COS0R2  +mR2lROlcR2COS  (0R1  +  0R2)  ] 

+  f,R:[,R2  +  mR2lcR2  +  mR2,RllcR2COS0R2  +  nV2,R»'cR2COS,0R1  +°R:»] 

+  »L]«L2(-2mL;lL1lcL2sin9LI-2inL2lLOlct  2sin(()u  +«,_,» | 

+  ‘»1.1  I"  '"liWcLI*'"6!.!  "  mL2,LOlL.si"°LI  "  2lI.»,cL2S'n  (°U  +  °L2>  I 
+  0L2[-'"L2,LIlcL2M"OL2-",L2lLOlcL2Sin<eu+Ol.2)l 
+  0RI0R2l_2mR2lRl1cR2sin0R2  “  2mR21ROl«R2S'n  (t)RI  +°R2)  I 
+  Ori  [-  >nRIlRolcRIsin0Ri  -  mR2lROlR|sin0R|  -  nnR2lROlcR sin  (0R|  +0R2)  ] 
2 

+  »R2 1"  mR2lR,lcR2sin0R2  -  mR2iROllR2sin  (0R,  +«R2)  I 


Any  polynomial  reference  trajectory  that  satisfies  the  initial  condition  concerning 
displacement  cannot  have  a  constant  term.  Polynomials  which  satisfy  the  velocity  and 

acceleration  initial  conditions  must  not  contain  linear  or  quadratic  terms.  The  general  n01 
order  polynomial  reference  trajectory  has  the  form 

f(x)  =  ant"  +  ajj.iT0'1  +  a^x”'2  +  ...  +  a5x5  +  a4x4  +  a3X3  (202) 

Derivatives  are 

f(x)  =  nanx"'1  +  (n-Da^jx"'2  +  (n-2)a„.2Xn'3  +  ...  +  5a5x4  +  4a4x3  +  3a3x2  (203) 
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I"  (x)  =  n(n- 1  ^x"'2  +  (n-l)(n-2)an.jx"'3  +  (n-2)(n-3)an.2t""4  +  ...  +  20a<x?  +  I2a4x4  +  6it3x  (204) 
When  x=  1  and  the  final  conditions,  f(  1 )  =  I ,  f(  1 )  =  0  nad  f  ( 1 )  =  0,  are  substituted 
into  Eqs.  (202)-(204),  these  equations  can  be  put  into  matrix  form 
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■  .  . 
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..  1  1  II 
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..5  4  ?| 

p] 

11  ( n  —  1 )  (n-  1)  (n-2) 
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..  20  12  r»j 

(205) 


The  column  vector  of  polynomial  coefficients  can  be  partitioned.  One  segment. 


3543,  contains  the  coefficients  for  the  third,  fourth,  and  fifth  order  terms  in  Eq.  (202).  The 
other  segment,  a^g^,  contains  all  of  the  coefficients  of  order  six  and  higher. 


a 


(206) 


The  W  matrix  can  be  partitioned  accordingly. 

w  =  K,gh  wmj]  (207) 

One  can  then  solve  for  the  lower  order  polynomial  coefficients  in  terms  of  the 
higher  order  coefficients  by  substituting  Eqs.  (206)  and  (207)  into  Eq.  (205).  The  result 
specifies  p^.  ynomial  reference  trajectory  coefficients  which  satisfy  the  boundary  condi¬ 
tions. 
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An  optimal  solution  for  a  polynomial  reference  trajectory  is  found  by  using  the 
MATLAB  function  fminu.  This  tool  numerically  finds  the  solution  to  an  unconstrained 
function  minimization  problem  using  a  quasi-Newton  method.  The  function  to  be  mini- 
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mized  is  the  rate  of  change  of  angular  momentum,  Eq  (201),  which  can  be  found  once  a 
reference  trajectory  is  specified.  The  user  makes  an  initial  guess  for  the  higher  order  refer¬ 
ence  trajectory  coefficients.  The  lower  order  coefficients  are  calculated  by  Eq.  (208).  The 
MATLAB  function  then  varies  the  higher  order  coefficients  and  recalculates  the  lower 
order  coefficients  as  necessary  to  minimize  change  in  angular  momentum.  One  limitation 
to  this  technique  is  that  the  algorithm  may  converge  to  a  local  rather  than  the  global  mini¬ 
mum 
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III.  VALIDATION  AND  SIMULATION  RESULTS 

The  computer  simulations  presented  in  this  chapter  were  obtained  using  the  MATLAB 
subroutines  included  in  Appendix  B.  The  integrator  uses  4th  and  5th  order  Runge-Kutta 
formulas  See  Appendix  B  for  documented  listings  of  the  computer  code. 

A.  VALIDATION 

To  verify  the  equations  and  find  the  programming  bugs,  test  cases  were  developed. 
The  simulations  are  analyzed  to  ensure  that  universal  principles  such  as  conservation  of 
energy  and  angular  momentum  are  not  violated  Numeric  values  for  the  generic  dual  two- 
link  manipulator  system  are  contained  in  Table  1 .  The  generic  model  is  the  strawman  con¬ 
figuration  that  all  of  the  test  cases  are  based  on  with  the  exception  of  a  few  minor  varia¬ 
tions.  The  variations  will  be  pointed  out  in  the  appropriate  test  cases.  The  values  for  the 
generic  model’s  system  properties  are  picked  for  uniformity  and  simplicity.  The  manipu¬ 
lator  links  and  the  payload  are  modelled  as  slender  rods  of  uniform  density. 

1.  Conservation  of  Kinetic  Energy 

In  the  first  test  case,  no  torques  are  applied  and  the  initial  velocities  are  nonzero. 
Under  these  conditions,  the  system  links  drift  subject  to  the  constraints  of  being  pinned 
together.  Since  potential  energy  is  zero  and  there  are  no  external  energy  sources,  kinetic 
energy  should  remain  constant.  The  system  begins  with  the  payload  parallel  to  a  line 
drawn  between  the  two  shoulders  and  0.75m  away  from  them.  The  initial  angular  rate  for 

the  centerbody  is  chosen  to  be  o0  =  2  deg/sec.  The  initial  angular  rate  for  the  payload  is  tip 
=  -5  deg/sec.  Initial  velocities  for  the  payload  center  of  mass  are  -0.1  m/sec  along  the  x 
axis  and  -0.05  m/sec  along  the  y  axis.  The  remaining  generalized  velocities  are  calculated 
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TABLE  1.  GENERIC  MODEL  SYSTEM  PROPERTIES 


based  on  the  values  specified  for  the  centerbody  and  payload  Initial  angular  rates  for  the 


manipulator  links  are  oL,  =  6.6607  deg/sec,  o, ,  =  -7  0457  deg/sec,  oRI  =  -2  7553  deg/sec, 

and  bR,  =  14  9127  deg/sec.  The  graphical  results  from  this  test  case  are  included  in  Fig¬ 
ures  4-8  As  indicated  in  Figure  7,  kinetic  energy  is  conserved  in  this  case. 


Figure  4:  Test  Case  1  Angies 


Figure  5:  Test  Case  1  Angular  Rates 
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Figure  8:  Test  Case  1  Angular  Momentum 

Test  Case  2  is  an  extension  of  Test  Case  1 .  This  is  still  a  case  with  nonzero  initial 
velocities  and  no  external  torques.  However,  the  system  geometry  is  altered  to  be  sym¬ 
metrical.  In  addition  to  conservation  of  kinetic  energy,  this  test  case  will  ensure  that  the 
symmetry  is  preserved.  T  he  physical  alterations  in  the  system  involve  moving  the  loca¬ 
tion  of  the  left  shoulder  from  90  degrees  to  135  degrees  and  decreasing  the  distance  from 
the  origin  to  the  right  shoulder  to  0.75  meters.  The  payload  still  begins  centered  between 
the  shoulders  and  parallel  to  the  y  axis  but  is  1 .2  m  from  the  origin.  To  maintain  symme¬ 
try,  the  initial  velocities  must  also  be  symmetrical.  The  initial  angular  rate  for  the  center- 

body  is  chosen  to  be  b0  =  0  deg/sec.  The  initial  angular  rate  for  the  payload  is  also  zero. 
Initial  velocities  for  the  payload  center  of  mass  are  zero  along  the  x  axis  and  -0.05  m/sec 
along  the  y  axis.  The  remaining  generalized  velocities  are  again  calculated  based  on  the 
values  specified  for  the  centerbody  and  payload.  Initial  angular  rates  for  the  manipulator 

links  are  dL1  =  2.3188  deg/sec,  eL2  =  -7.6851  deg/sec,  0R,  =  -2.3188  deg/sec,  and  (iR2  = 
7.6851  deg/sec.  This  combination  of  system  geometry  and  initial  velocities  is  designed  to 


0  L 
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cause  the  payload  to  drift  toward  the  origin  without  changing  its  attitude.  Figures  9-13 
show  the  results  from  this  test  case.  Kinetic  energy  is  conserved  and  symmetry  is  pre¬ 
served 


0  5  10 
Time  (sec) 


Figure  9:  Test  Case  2  Angles 
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Figure  10:  Test  Case  2  Angular  Rates 
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Figure  13:  Test  Case  2  Angular  Momentum 

2.  Conservation  of  Angular  Momentum 

As  long  as  a  system  does  not  include  external  torques,  one  expects  that  angular 
momentum  should  be  conserved.  The  joint  actuators  provide  internal  torques  while  the 
reaction  wheel  is  the  only  external  source.  Test  Cases  1  and  2  did  not  include  a  reaction 
wheel  and  are  therefore  subject  to  investigation  with  respect  to  conservation  of  angular 
momentum.  Both  cases  do  satisfy  the  requirement  as  indicated  by  Figures  8  and  13.  Fur¬ 
thermore,  due  to  the  symmetry  in  the  system  in  Test  Case  2,  the  angular  momentum  of  the 
left  manipulator  links  should  be  cancelled  out  by  the  angular  momentum  of  the  right 
manipulator  links.  The  centerbody  and  payload  should  not  have  any  angular  momentum. 
Consequently,  angular  momentum  for  the  system  should  not  only  be  conserved,  it  should 
be  zero.  Figure  13  show  that  the  angular  momentum  remained  virtually  zero.  The  non¬ 
zero  values  of  about  3xl0"17  are  well  within  the  integration  algorithm  tolerance  of  lO*6 

Test  Case  3  returns  to  the  generic  system  from  Table  1 .  Initially,  the  system  is  at 
rest.  Constant  torques  are  applied  at  both  shoulders  and  nowhere  else.  The  torques  are 
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0  0 1  N-m  applied  in  the  positive  direction  at  the  right  shoulder  and  the  negative  direction 
at  the  left  shoulder.  Because  the  joint  torques  are  internal  to  the  system,  angular  momen¬ 
tum  must  still  be  conserved  even  though  kinetic  energy  won’t  be  Furthermore,  since  the 
system  began  at  rest,  the  angular  momentum  should  remain  at  zero  The  results  are  shown 
in  Figures  14-18  Although  the  angular  momentum  did  not  remain  identically  equal  to 

zero,  their  magnitudes  of  less  than  2x1  O'7  are  within  the  l O'6  tolerance  placed  on  the  inte¬ 
gration  algorithm. 


Time  (sec) 

Figure  14:  Test  Case  3  Angles 
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Figure  15:  Test  Ca! 


Figure  16:  Test  Case  3  1 


Figure  17:  Test  Case  3  Kinetic  Energy 


Figure  18:  Test  Case  3  Angular  Momentum 


Test  Case  4  is  similar  to  Test  Case  3  but  the  symmetrical  system  geometry  is  used 
instead  of  the  generic  geometry.  This  change  should  produce  symmetric  motion  and  zero 
angular  momentum.  The  reaction  wheel  is  still  disabled.  Figures  19-23  indicate  the  sys- 
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tem  reacted  as  expected.  Changing  the  torques  to  time  varying  profiles  rather  than  con¬ 
stants  led  to  similar  results 


Figure  19:  Test  Case  4  Angies 


Figure  20:  Test  Case  4  Angular  Rates 
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Figure  22:  Test  Case  4  Kinetic  Energ) 
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Figure  23:  Test  Case  4  Angular  Momentum 

3.  Wheel  Torque  and  Constraints 

The  remaining  test  cases  involved  using  the  reaction  wheel  on  the  centerbody.  The 
wheel’s  function  was  to  maintain  attitude  pointing.  The  system  begins  at  rest.  The  torque 
applied  by  the  wheel  is  an  external  torque  in  this  model.  Therefore,  its  value  must  be  the 
same  as  the  change  in  angular  momentum.  The  wheel  torque  is  found  by  means  of  the 
inverse  kinematics  equations  in  Chapter  II.  These  calculations  are  entirely  independent  of 
finding  the  change  in  angular  momentum.  After  a  simulation  is  finished,  a  separate  sec¬ 
tion  in  the  program  code  calculates  the  change  in  angular  momentum  using  the  general¬ 
ized  coordinates,  velocities  and  accelerations  produced  by  the  integration.  These  values 
are  plotted  along  with  those  of  the  reaction  wheel  torque.  A  sample  plot  is  contained  in 
Figure  24.  This  particular  plot  is  for  the  case  of  a  fifth  order  polynomial  reference  trajec¬ 
tory.  The  rest  of  the  plots  associated  with  this  case  are  presented  later  in  the  Simulations 
section.  The  validation  tests  concerning  conservation  of  kinetic  energy  and  angular 
momentum  required  special  circumstances  to  create  those  conditions.  The  requirement 
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that  the  reaction  wheel  torque  equal  the  change  in  angular  momentum  is  more  universal. 
It  is  a  verification  check  performed  with  every  simulation  involving  a  reaction  wheel. 
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Figure  24:  Sample  of  Wheel  Torque  and  Change  in  Angular  Momentum  vs.  Time 


An  even  more  universal  check  also  performed  with  every  simulation  is  the  require¬ 
ment  that  the  constraint  equations  (Aq  +  A(J  =  o)  are  satisfied.  Figure  25  shows  a  sample 
plot.  This  plot  was  also  taken  from  the  fifth  order  polynomial  reference  trajectory  case. 
The  values  plotted  represent  the  four  constraint  equations  contained  in  Eqn  72.  The  non¬ 
zero  values  are  attributed  to  numerical  errors  created  by  the  integration. 

Finally,  a  common  sense  check  also  performed  with  every  simulation  is  simply  to 
verify  that  the  payload  was  repositioned  to  the  desired  final  location.  This  cannot  happen 
if  the  torques  applied  to  the  system  were  incorrect.  This  test  is  a  necessary  but  not  suffi¬ 
cient  condition  that  the  code  operates  correctly. 
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Figure  25:  Sample  or  Constraints  vs.  Time 


B.  SIMULATIONS 

This  section  presents  results  from  several  simulations  of  an  analytical  model.  The 
desired  payload  repositioning  maneuver  is  illustrated  in  Figure  26.  The  final  position  for 
the  payload  involves  a  90  degree  rotation  and  the  right  endpoint  finishes  where  the  left 
endpoint  started. 

1 .  Lyapunov  Point  Controller 

In  the  first  simulation,  the  repositioning  is  done  entirely  by  the  Lyapunov  control¬ 
ler  without  the  benefit  of  a  reference  trajectory.  The  behavior  is  that  of  a  point  controller 
with  an  initial  displacement  rather  than  that  of  a  tracking  controller.  Due  to  the  absence  of 
the  weighted  norm  reference  torques,  this  controller  cannot  be  consider  to  have  coopera¬ 
tive  nature.  Figure  27  presents  the  angular  displacement  history.  The  asterisks  on  the 
right  side  of  the  plot  indicate  the  desired  final  angles.  Although  the  system  is  approaching 
the  desired  final  geometry,  it  has  not  completely  settled  down  even  after  40  seconds.  Posi¬ 
tion  errors  (Figure  28)  are  still  present  as  well  as  nonzero  velocities  (Figure  29).  Also  note 
that  the  reaction  wheel  torque  is  quite  high  during  the  maneuver  (Figure  30).  The  joint 
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actuator  torques  are  considerably  less  than  the  reaction  wheel  torque.  They  are  not  identi¬ 
fied  individually  because  the  most  important  feature  of  Figure  30  is  the  reaction  wheel 

torque  As  a  quantitative  measure  of  this  controller’s  quality,  J j  uwh| di  produces  a  value  of 

17  3841  The  oscillatory  nature  of  the  system  is  evident  in  the  angular  position  and  veloc¬ 
ity  plots.  Despite  the  oscillations,  however,  the  stability  of  the  controller  is  also  illus¬ 
trated  Figure  3 1  depicts  the  system  geometry  at  several  instances  during  the  maneuver 
The  left  manipulator  links  actually  cross  over  each  other  In  experimental  hardware,  the 
links  would  collide  instead.  Figure  32  removes  the  clutter  that  is  present  in  Figure  3 1  and 
displays  only  the  initial  and  final  geometry  The  Lyapunov  point  controller  also  does  a 
poor  job  of  maintaining  the  centerbody  attitude.  This  is  clearly  evident  in  Figures  2n  and 
3 1 .  The  attitude  error  peaks  at  about  16  degrees. 


Figure  26:  Desired  Repositioning  Maneuver 
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Figure  27:  Lyapunov  Point  Controller  Angles 


Displacement  Errors  vs  Time 


Figure  28:  Lyapunov  Point  Controller  Displacement  Errors 
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ThctalXiis  vs  i  ime 


Figure  29:  Lyapunov  Point  Controller  Angular  Rates 
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Figure  30:  Lyapunov  Point  Controller  Command  Torques 
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2.  Lyapunov  Tracking  Controller 


This  controller  uses  the  following  equation  to  calculate  control  torques 


=  ct{-K5q  +  C.  u  ..-(C,q-C,  q  )  -  (C,  -  C,  )-Kp5qJ  (209) 

1  '  ret  -  -,<•(  ’ret 


This  equation  was  developed  in  the  analytical  chapter  and  repeated  here  tor  conve¬ 
nience  The  command  torques  are  based  on  errors  with  a  reference  trajectory  Reference 
torques  which  resulted  from  minimizing  a  weighted  norm  of  the  actuator  torques  associ¬ 
ated  with  the  reference  trajectory  are  also  included. 
a.  5th  Order  Reference  Trajectory 

In  this  simulation,  a  fifth  order  polynomial  reference  trajectory  is  applied  to  the 
payload  generalized  coordinates.  The  payload  coordinates  displacements,  velocities,  and 
accelerations  resulting  from  this  polynomial  are  depicted  in  Figure  33.  When  calculating 
the  reference  torques  from  the  inverse  kinematics,  the  six  joint  actuators  are  all  weighted 
equally.  The  maneuver  time  is  selected  to  last  10  seconds.  As  is  demonstrated  in  Figures 
34-36,  the  system  successfully  moves  from  initial  conditions  to  desired  final  conditions. 
The  displacement  errors  are  less  than  10'4  deg  (Figure  34).  The  command  torques  (Figure 
37)  are  an  order  of  magnitude  smaller  than  for  the  previous  simulation  which  lacked  a 
reference  trajectory.  Evaluating  J|uwh|dt  leads  to  the  dramatically  improved  value  of 
0  5746  More  importantly,  the  centerbody  attitude  is  maintained  throughout  the  maneuver. 
Figure  38  shows  the  time  lapse  depiction  of  the  maneuver. 
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Figure  33:  5th  Order  Reference  Trajectories 
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Figure  34:  5th  Order  Angies 
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Figure  35:  5th  Order  Displacement  Errors 
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Figure  36:  5tb  Order  Angular  Rates 
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Figure  37:  5th  Order  Command  Torques 


Figure  38:  5th  Order  Time  Lapse  Stick  Figure 


b.  H,h  Order  Reference  Trajectory 

By  increasing  the  order  of  the  reference  trajectory  polynomial  while 
maintaining  the  same  boundary  conditions  concerning  velocity  and  acceleration,  one 
hopes  to  achieve  improved  performance  For  example,  the  domain  of  all  sixth  order 
polynomial  functions  includes  all  fifth  order  polynomial  functions  as  a  subset.  Therefore, 
when  searching  all  sixth  order  polynomials  for  coefficients  which  will  minimize  the  cost 
function,  one  possible  solution  is  the  fifth  order  polynomial  already  used  Using  the 
function  minimiza'on  routine  discussed  in  the  previous  chapter,  a  sixth  order  polynomial 
function  was  found  Although  there  was  some  improvement,  the  change  in  performance 
was  not  significant  The  same  was  true  for  a  seventh  order  function  An  eighth  order 
function  is  presented  here.  It  was  hoped  that  the  increased  order  would  be  enough  of  a 
departure  from  the  fifth  order  cause  to  produce  significant  improvement  in  reducing  the 
centerbody  disturbance  torque.  The  algorithm  converged  to  a  solution  for  the  eighth  order 
polynomial  after  running  approximately  two  hours  on  a  personal  computer  with  an  Intel 
486-DX50  cpu.  The  resulting  trajectories  are  very  similar  to  those  for  the  fifth  order  case 
and  are  displayed  in  Figure  39.  The  most  obvious  difference  is  a  lack  of  symmetry.  Plots 
for  this  case  are  contained  in  Figures  40-43.  The  value  of  J|uwh;dt  for  this  case  was  0.5705. 
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Figure  39:  8th  Order  Reference  Trajectories 
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Figure  40:  8th  Order  Angies 
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Figure  41:  8th  Order  Angular  Rates 


Figure  42:  8th  Order  Command  Torques 
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Figure  43:  8th  Order  Time  Lapse  Stick  Figure 

3.  Modified  Lyapunov  Tracking  Controller 

This  simulation  represents  a  compromise  between  the  Lyapunov  point  controller 
and  the  Lyapunov  tracking  controller.  Because  the  Lyapunov  point  controller  does  not  use 
a  reference  trajectory,  the  cost  function  which  minimizes  the  weighted  norm  of  the  actua¬ 
tor  torques  is  completely  bypassed.  The  modified  Lyapunov  controller  removes  the  refer¬ 
ence  torque  term  from  the  command  torque  calculation  (Eqn  209)  but  calculates  command 
torques  based  on  errors  with  a  reference  trajectory.  Like  the  Lyapunov  point  controller, 
the  modified  Lyapunov  tracking  controller  does  not  minimize  a  weighted  norm  of  the 
actuator  torques  and  is  therefore  not  a  cooperative  controller.  The  angle  histories  in  Figure 
44  exhibit  less  of  the  oscillatory  nature  than  the  point  controller  simulation,  but  the  accu¬ 
racy  shown  in  Figure  45  is  considerably  worse  than  the  reference  trajectory  simulations. 
Figures  46-48  also  illustrate  behavior  better  than  the  point  controller  but  not  as  good  as 
when  command  torques  are  found  using  Eqn  209  The  magnitude  of  the  command  torques 
show  an  order  of  magnitude  improvement  over  the  point  controller.  This  is  directly  attrib¬ 
utable  to  using  intermediate  reference  points  on  the  way  to  a  desired  final  state  rather  than 
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attempting  to  achieve  the  desired  final  state  all  at  once  Calculating  |juwh|dt  produced  a 

value  of  2.4523.  The  time  lapse  figure  shows  that  the  motion  is  much  less  wild  but  the 
centerbody  attitude  error  is  still  noticeable 


0  20  40 

Time  (sec) 

Figure  44:  Modified  Lyapunov  Tracking  Controller  Angles 


Displacement  Frrors  vs  Time 


Figure  45:  Modified  Lyapunov  Tracking  Controller  Displacement  Errors 
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Theta  Dots  vs  Time 


Figure  46:  Modified  Lyapunov  Tracking  Controller  Angular  Rates 


Command 

Torques 


Command  Torques  vs  Time 


Figure  47:  Modified  Lyapunov  Tracking  Controller  Command  Torques 
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Figure  48:  Modified  Lyapunov  Tracking  Controller  Time  Lapse  Stick  Figure 

4.  Comparison  of  Controllers 

Table  2  summarizes  the  results  of  the  Lyapunov  point  controller,  the  two  Lyapunov 
tracking  controller  cases,  and  the  modified  Lyapunov  tracking  controller.  The  point  con¬ 
troller  clearly  has  the  worst  performance  with  high  reaction  wheel  torque  and  large  center- 
body  attitude  error.  The  tracking  controller  performs  much  better.  Reaction  wheel  torque 
is  greatly  reduced  and  centerbody  attitude  error  is  eliminated.  As  expected,  increasing  the 
order  of  the  polynomial  reduces  the  reaction  wheel  torque  further,  but  the  improvement  is 
relatively  small.  The  modified  tracking  controller  strikes  a  compromise  between  the  point 
controller  and  the  tracking  controller. 
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TABLE  2.  COMPARISON  OF  HYPOTHETICAL  MODEL 
SIMULATIONS 


llvJ* 

U 

man 

Centerbody 
Attitude 
Error  (deg) 

Cooperative 

Point  Controller 

17.3841 

2  9365 

16  2261 

No 

Tracking 

Controller 

5Ul  Order 

0.5746 

Yes 

8th  Order 

0.5705 

00885 

0.0000 

Yes 

Modified  Tracking 
Controller 

2.4523 

0  3950 

1.1910 

No 
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IV.  EXPERIMENTAL  WORK 


The  experimental  phase  of  this  research  was  conducted  on  the  Spacecraft  Robotics 
Simulator  (SRS).  The  SRS  is  a  derivative  of  the  Flexible  Spacecraft  Simulator  (FSS)  ini¬ 
tially  developed  by  Watkins  [Ref  17]  and  later  modified  by  Hailey  [Ref  18].  Sorensen 
[Ref  1 8]  began  the  work  to  convert  the  FSS  into  the  SRS. 

A.  SETUP 

The  SRS  permits  experimental  investigation  of  two  dimensional  robotics  motion  and 
rotational  spacecraft  dynamics.  The  SRS  is  illustrated  in  Figures  49  and  50.  The  simula¬ 
tor  hardware  is  floated  on  an  eight  foot  by  six  foot  granite  table  by  means  of  a  thin  layer  of 
air  supplied  by  an  external  source.  The  table  is  polished  to  within  0  001  inch  peak  to  val¬ 
ley  and  leveled  to  prevent  gravitational  accelerations  from  impacting  the  motion  across  its 
surface.  The  following  sections  describe  the  simulated  spacecraft  with  its  associated  sen¬ 
sors  and  actuators  and  the  controller  which  together  form  the  SRS.  The  spacecraft  compo¬ 
nents  are  the  centerbody,  two  manipulators,  and  a  payload. 
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Figure  49:  Spacecraft  Robotics  Simulator 
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1 .  Centerbody 

The  centerbody  is  a  30  inch  diameter,  7/8  inch  thick  aluminum  disk.  The  center- 
body  carries  a  position  sensor,  rate  sensor,  momentum  wheel,  thrusters,  and  an  air  tank  to 
power  the  thrusters.  The  centerbody  also  serves  as  the  mounting  platform  for  the  manipu¬ 
lators.  The  centerbody  is  floated  by  a  central  air  bearing  and  three  air  pads  located  at  120 
degree  intervals  near  the  outer  edge.  The  air  pads  are  each  capable  of  floating  60  pounds 
when  the  air  pressure  supplied  to  the  pads  is  80  psi.  The  air  bearing  is  attached  to  an  over¬ 
head  I-beam  which  restricts  to  motion  of  the  centerbody  to  rotation  only. 
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Centerbody  angular  position  is  sensed  by  a  Rotary  Variable  Displacement  Trans¬ 
ducer  (RVDT)  mounted  directly  above  the  air  bearing  The  RVDT  is  a  model  R30D  man¬ 
ufactured  by  Schaevitz  Sensing  Systems  Its  linear  range  is  restricted  to  ±  40  degrees. 
Centerbody  angular  rate  is  measured  by  a  rate  transducer  manufactured  by  Humphrey,  Inc. 
The  instrument  has  a  range  of  ±100  deg/sec  and  a  minimum  threshold  of  0  01  deg/sec 

Centerbody  angular  position  is  controlled  by  a  momentum  wheel  The  momentum 
wheel  speed  is  measured  by  a  tachometer  contained  in  the  servo  motor  which  drives  the 
momentum  wheel.  The  centerbody  momentum  wheel  is  powered  by  a  model  JR16M4CH/ 
F9T  servo  motor  manufactured  by  PMl  Industries.  Characteristics  of  this  motor  are  sum¬ 
marized  in  Table  3.  Although  the  centerbody  also  carries  two  thrusters,  they  are  not  used 
in  this  research. 


TABLE  3.  MOMENTUM  WHEEL  MOTOR 
CHARACTERISTICS 


Manufacturer 

PMI  Industries 

Model 

JRI6M4CH/F9T 

Rated  Output  Speed  (rpm) 

3000 

Rated  Current  (amps) 

7  79 

Rated  Voltage  (volts) 

168 

Rated  Torque  (in-lb) 

31  85 

Height  (in) 

4  5 

Weight  (lb) 

17.5 

Outside  Diameter  (in) 

7.4 

2.  Manipulators 

Two  two-link  manipulators  are  mounted  60  degrees  apart  on  the  centerbody.  Each 
manipulator  has  three  joints.  The  shoulder  joints  are  supported  by  the  centerbody  while 
the  elbow  and  wrist  joints  are  supported  by  two  air  pads  apiece.  The  links  between  the 
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joints  are  stiff  laterally  but  permit  some  flexibility  vertically  This  feature  increases  the 
tolerances  on  the  air  pad  height  adjustment 


Left  arm  joint  angles  are  measured  by  the  same  model  RVDT  as  is  used  on  the  cen 
terbodv  All  three  of  the  left  arm  actuators  are  senes  9FGHD  servo  disk  motors  manufac¬ 
tured  by  PMI  Industries.  Joint  angles  on  the  right  arm  are  sensed  by  encoders  purchased 
with  the  joint  actuators.  The  encoder  resolution  is  0  005  decrees.  The  right  arm  joint 
actuators  arm  are  harmonic  drive  dc  servo  actuators  manufactured  by  HD  Systems,  Inc. 
The  shoulder  actuator  is  model  RFS-25-6018-E036AL  while  the  elbow  and  wnst  actua¬ 
tors  are  model  RFS-20-60 1 2-E036AL.  Specifications  for  the  three  types  of  joint  actuators 
are  contained  in  Table  4. 


Figure  51:  Left  Manipulator  Top  and  Side  Views 
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Figure  52:  Right  Manipulator  Top  and  Side  Views 


TABLE  4.  MANIPULATOR  ACTUATOR  CHARACTERISTICS 


Manufacturer 

HD  Systems 

HD  Systems 

PMI  Industries 

Model 

BaBa 

9FGHD 

Reduction  Ratio 

1:50 

1.50 

1:148.5 

Rated  Output  Speed  (rpm) 

60 

60 

17 

Rated  Current  (amps) 

2.9 

3.9 

5.6 

Rated  Voltage  (volts) 

75 

75 

12 

_i 

Rated  Torque  (in-lb) 

174 

260 

80 

i 

Height  (in) 

8.8 

9.6 

3 

Weight  (lb) 

9.3 

14.1 

3.2 

Footprint  (in) 

hbeshh 

BUI 

4.8(2) 

1  Side  of  square 

2  Diameter  of  circle 
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The  joint  actuators  are  all  driven  by  Kepco  power  supplies.  These  bipolar,  pro¬ 
grammable,  linear  amplifiers  can  be  controlled  manually  from  the  front  panel  or  con¬ 
trolled  remotely  with  a  ±10  volt  signal.  In  this  application,  the  power  supplies  are 
operated  in  the  current  control  mode  with  the  voltage  and  current  limits  manually  set  con¬ 
sistent  with  the  values  in  Table  4.  The  specific  power  supply  models  and  their  characteris¬ 
tics  are  summarized  in  Table  5. 


TABLE  5.  POWER  SUPPLIES  CHARACTERISTICS 


Model 

BOP  72-6M 

BOP  72-3M 

BOP  20-10M 

Actuators  Controlled 

Right  Shoulder 

Right  Elbow, 
Right  Wrist 

All  Left  Arm 
Joints 

DC  Output  Range 

±72  volts 
±6  amps 

±72  volts 
±3  amps 

±20  volts 
±10  amps 

Closed  Loop  Gain 

0.6  (amp/volt) 

0.3  (amp/volt) 

1 .0  (amp/volt) 

3.  Payload 

The  payload  is  a  rigid  bar  mechanically  fastened  to  the  ends  of  both  manipulators. 
The  payload  is  supported  entirely  by  the  air  pads  on  the  manipulator  wrist  joints.  Ballast 
can  be  added  to  the  payload  to  change  the  mass  and  inertia  characteristics  of  the  system. 
This  allows  for  the  construction  of  cases  in  which  the  mass  of  the  payload  is  nontrivial 
compared  to  the  spacecraft  centerbody.  The  payload  contains  no  sensors  or  actuators. 
Payload  position  is  derived  from  the  manipulator  joint  angles. 

4.  Controller 

The  AC-100  programmable  controller  manufactured  by  Integrated  Systems,  Inc. 
controls  the  SRS.  The  AC-100  includes  an  Intel  80386  Application  Processor,  an  Intel 
80386  Multibus  II  Input/Output  Processor,  an  Intel  80386  Communication  Processor,  and 
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Intel  80387  Coprocessor,  a  Weitek  3167  Coprocessor,  and  Anaiog-To-Digitai  and  Digital- 
To-Analog  Data  Translation  DT2402  Input/Output  Board,  two  INX-04  Encoder  and  Digi- 
tal-To-Analog  Servo  Boards,  and  an  Ethernet  Interface  Module.  The  AC- 100  also 
includes  software  installed  on  a  VAX  3100  Series  Model  30  workstation.  The  software 
permits  design  of  a  controller  in  block  diagram  graphical  form  and  conversion  of  the  dia¬ 
gram  to  C  language  programming  code.  The  user  is  also  able  to  design  an  interactive  ani¬ 
mation  window  to  operate  the  controller.  The  AC- 100  receives  input  signals  from  the 
sensors  and  the  graphical  user  interface.  AC- 100  output  signals  go  to  the  power  supplies 
driving  the  actuators  or  to  the  graphical  user  interface  for  display. 

S.  System  Integration 

The  differences  between  the  ideal  world  of  an  analytical  simulation  and  the  real 
world  of  actual  hardware  became  apparent  during  system  integration.  A  few  problems 
arose  then  requiring  some  modification  of  the  experiment.  The  first  problem  concerned 
floating  the  centerbody.  It  exhibited  a  noticeable  resistance  to  rotation.  This  is  due  in  part 
to  the  air  pressure  of  the  available  air  supply.  Because  it  was  only  40  psi,  the  air  pads  per¬ 
formance  was  degraded  by  a  factor  of  two.  Prior  to  mounting  the  manipulators,  the  cen¬ 
terbody  weighed  approximately  125  lbs.  Adding  the  shoulder  motors  increased  the 
centerbody  weight  to  145  lbs.  The  extra  weight  may  have  been  enough  to  overwhelmed 
the  centerbody  air  pads.  A  second  contributing  factor  to  the  centerbody  drag  is  the  inabil¬ 
ity  of  the  central  air  bearing  to  function  except  under  very  low  lateral  loading.  The  modi¬ 
fication  to  the  experiment  created  by  the  centerbody  problem  is  to  not  float  the  centerbody. 

A  second  problem  involved  using  the  RVDTs.  As  envisioned,  the  experiment 
requires  one  RVDT  for  the  centerbody  and  three  for  the  left  manipulator  joints.  The  Space 
Dynamics  laboratory  has  a  total  of  three  in  stock.  Although  a  fourth  has  been  ordered,  it 
did  not  arrive  in  time  to  be  used.  Using  the  existing  RVDTs  revealed  another  problem. 
Data  acquisition  of  the  RVDT  signal  by  the  AC-100  exhibited  a  random  toggling  of  the 


sensed  value  between  a  good  reading  and  a  value  of  zero.  Because  the  angle  information 
is  critical  to  calculating  actuator  commands,  this  behavior  is  unacceptable.  Consultation 
with  the  Integrated  Systems  technical  support  group  revealed  that  this  type  of  behavior  is  a 
bug  within  the  AC- 100  software  which  has  been  corrected  in  more  recent  versions.  Use  of 
the  newer  version  was  not  possible  because  it  requires  upgrading  the  VAX  workstation 
hardware  and  an  updated  version  of  the  VMS  operating  system  The  experimental  modifi¬ 
cation  used  to  overcome  these  difficulties  is  to  derive  the  joint  angles  and  velocities  of  the 
left  manipulator  by  using  the  sensed  information  from  the  right  manipulator  encoders. 
Velocities  were  not  sensed  directly  but  approximated  by  the  change  in  displacement  which 
occurred  since  the  last  sample  divided  by  the  sample  rate 

A  third  obstacle  involved  the  limitations  of  software  to  design  the  control  algo¬ 
rithm  The  block  diagram  construction  method  did  not  permit  convenient  matrix  opera¬ 
tions.  Matrix  multiplication  must  be  programmed  in  an  element  by  element  basis.  Matrix 
inversion  must  also  be  calculated  by  constructing  a  series  of  blocks  to  find  each  element. 
This  handicap  is  not  serious  when  the  system  equations  of  motion  are  of  low  order.  How¬ 
ever,  the  dual  two  link  manipulator  configuration  is  eighth  order  and  beyond  the  practical 
means  of  programming  complex  matrix  operations,  especially  matrix  inverses.  Recall  that 
the  command  torques  are  calculated  by  the  following  relationship 
u  =  (c[c,f'c[  {-Kv8q+C,  ure(-  <C2q  - C2> -  (C,  -  C3  J  -  Kp6q) 

(210) 

When  the  differences  between  the  actual  path  and  the  reference  path  are  small,  this  control 
law  simplifies  to  something  very  similar  to  a  PD  controller.  Therefore,  the  control  law 
used  by  the  experiment  is  a  PD  controller  rather  than  the  complete  Lyapunov  controller. 

Performance  differences  between  the  left  and  right  manipulator  actuators  also  pre¬ 
sented  some  problems.  Because  of  the  actuator  redundancy,  any  three  joint  actuators 
should  be  enough  to  follow  a  reference  trajectory.  This  fact  can  be  demonstrated  by  using 
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only  the  three  right  joint  actuators.  However,  the  same  trajectory  is  not  possible  with  only 
the  left  actuators.  The  torque  provided  by  the  left  joint  actuators  is  insufficient  to  com¬ 
pletely  overcome  the  internal  friction  of  the  right  joint  actuators.  Even  when  the  left  joint 
actuators  are  commanded  manually  from  the  front  panel,  there  is  no  correction  to  reduce 
the  position  error.  When  steadily  increasing  the  commanded  current  to  the  motor,  the  cur¬ 
rent  limit  is  reached  before  the  motor  responds 

B.  RESULTS 

The  reference  trajectory  for  the  experimental  phase  is  slightly  different  from  that  used 
in  the  analytical  section.  The  reference  maneuver  still  involves  a  90  degree  rotation  of  the 
payload  with  the  right  endpoint  ending  wh-jre  the  left  endpoint  began.  The  differences 
arise  from  the  system  parameter  such  as  lengths  and  masses  not  being  the  same  as  in  the 
generic  hypothetical  model.  The  desired  reference  maneuver  is  depicted  in  Figure  53. 
Results  are  shown  in  Figures  54-58  and  summarized  in  Table  6.  The  sudden  changes  from 
believable  values  to  zero  in  the  figures  are  problems  with  the  data  acquisition  software  and 
do  not  indicate  actual  changes  in  the  experimental  hardware  geometry. 


Figure  53:  Desired  Experimental  Repositioning  Maneuver 
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Figure  54:  6p  Commanded,  Actual,  and  Error  Angles  vs  Time 


O  / 


in  o  mo  m  o  m  o  mio  ^  n  o  «  ^  « 

'  f*  »-i-  '  r  r  ’  l  i  t 


(Bap)  puio  im 


(Bap)  ITU 


(Bap)  ng  nm 


Figure  55:  0L|  Commanded,  Actual,  and  Error  Angles  vs  Time 
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Figure  56:  9^2  Commanded,  Actual,  and  Error  Angles  vs  Time 
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Figure  57:  0R1  Commanded,  Actual,  and  Error  Angles  vs  Time 
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Figure  58:  0R2  Commanded,  Actual,  and  Error  Angles  vs  Time 
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Tima  (sac) 


TABLE  6.  EXPERIMENTAL  ERROR  ANGLES 


Errors  (deg) 

Initial 

Final 

Maximum 

Magnitude 

Op 

-0.3383 

0.5527 

©LI 

0.0366 

0.7797 

0L2 

0.1873 

03035 

0R1 

■ 

-0.0808 

0  1628 

9R2 

0.3350 

0  3950 

0.7742 
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V.  SUMMARY  AND  CONCLUSIONS 

A.  SUMMARY 

The  dynamics  of  a  dual  two-link  manipulator  system  which  is  repositioning  an  already 
grasped  payload  have  been  analyzed.  The  equations  of  motion  for  the  system  were  devel¬ 
oped  using  Lagrange’s  method  The  resulting  equations  were  highly  noni/near,  coupled, 
second  order  differential  equations  Given  any  reference  trajectory,  the  actuator  torques 
that  will  produce  that  trajectory  were  calculated  to  minimize  a  weighted  norm  of  the 
torques.  Stability  of  the  system  during  the  repositioning  maneuver  was  ensured  by  a  con¬ 
troller  derived  from  Lyapunov  stability  theory.  Equations  for  deriving  joint  angles  from 
centerbody  and  payload  reference  values  was  alsr  developed.  Polynomial  reference  tra¬ 
jectories  were  presented  as  an  attractive  means  to  specify  a  reference  trajectory. 

The  analytical  model  was  validated  using  test  cases  in  which  some  results  could  be 
predicted  in  advance.  The  model  demonstrated  conservation  of  energy  when  no  torques 
were  applied.  It  also  exhibited  conservation  of  angular  momentum  whenever  the  reaction 
wheel  was  disabled.  The  model  also  maintained  symmetric  geometry  in  the  appropriate 
test  cases.  In  cases  which  used  the  reaction  wheel,  conservation  of  energy  and  angular 
momentum  did  not  apply,  However,  comparison  of  the  change  in  angular  momentum  with 
the  reaction  wheel  torque  provided  validation.  Finally,  in  all  test  cases  as  well  as  simula¬ 
tions,  the  constraints  were  satisfied  as  measured  by  Aq  +  a()  =  <>. 

Results  from  simulations  indicated  that  the  Lyapunov  point  controller,  although  stable, 
behaved  poorly.  Large  centerbody  attitude  errors,  high  command  torques,  and  wild  oscil¬ 
lations  make  this  controller  undesirable  for  large  ^positioning  maneuvers.  The  Lyapunov 
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tracking  controller  exhibited  dramatic  a  improvement  in  performance.  Centerbody  atti¬ 
tude  errors  were  removed  and  reaction  wheel  torque  decreased  significantly 

The  experimental  phase  revealed  that  the  controller  required  further  simplification  for 
compatibility  with  the  laboratory  resources  Acceptable  results  were  obtained  using  a  PD 
control  law  with  a  reference  trajectory 

The  objectives  of  this  research  were  to  I )  develop  a  stable  control  law  that  facilitates 
cooperation  among  the  manipulators  as  they  reposition  the  payload,  2)  minimize  the  joint 
actuator  effort,  3)  reduce  the  disturbance  torque  transmitted  to  the  spacecraft  main  body 
by  the  manipulator  motion,  and  4)  validate  the  analytical  development  with  experimental 
results  The  Lyapunov  controller  satisfies  the  first  objective  The  second  objective  is 
achieved  by  the  weighted  norm  calculation  of  the  actuator  torques  Reduction  of  the  cen¬ 
terbody  disturbance  torque  is  accomplished  through  reference  trajectory  selection. 
Although  a  rigorous  application  of  classical  optimal  control  techniques  proved  impracti¬ 
cal,  a  polynomial  reference  trajectory  in  which  the  coefficients  were  selected  to  reduce  the 
disturbance  torque  was  easily  applied  Difficulties  were  encountered  with  regards  to  the 
fourth  objective,  experimental  work.  The  controller  developed  analytically  could  not  be 
directly  transferred  to  the  laboratory.  This  was  due  to  a  combination  of  hardware  limita¬ 
tions  and  real  world  conditions  instead  of  the  ideal  environment  of  the  analytical  model. 
The  controller  was  adapted  to  the  realities  of  the  laboratory  and  resulted  in  successful 
accomplishment  of  a  payload  repositioning  maneuver. 

B.  ORIGINAL  CONTRIBUTIONS 

A  simulation  tool  has  been  developed  to  analyze  the  dynamics  of  a  space  based  robot¬ 
ics  system.  Some  of  the  features  of  this  tool  include: 
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(i)  rotational  motion  of  the  spacecraft  centerbody  and  planar  motion  of  the  manip¬ 
ulators  and  payload, 

(ii)  minimization  of  a  weighted  norm  of  the  actuator  torques  based  on  a  user  sup¬ 
plied  weighting  matrix; 

(iii)  calculation  of  polynomial  reference  trajectory  coefficients  to  produce  a  local 
minimum  for  the  integral  of  the  absolute  value  of  the  disturbance  torque  based 
on  a  user  supplied  order  for  the  reference  polynomial  and  an  initial  guess  for 
the  coefficients; 

(iv)  a  reference  trajectory  with  zero  velocity  and  acceleration  at  the  beginning  and 
end  of  the  maneuver; 

(v)  a  Lyapunov  controller  which  guarantees  stability  in  the  face  of  perturbations 
between  the  reference  trajectory  and  the  actual  dynamics  caused  by  errors  in 
the  initial  conditions. 

An  experimental  test  bed  was  also  developed.  This  effort  involved  the  design  of  the 
manipulator  components  and  the  development  of  a  real  time  controller.  This  test  bed 
remains  in  the  Spacecraft  Dynamics  and  Control  Laboratory  and  is  available  for  follow-on 
work. 

C.  RECOMMENDATIONS  FOR  FURTHER  STUDY 

As  with  any  research,  this  work  answers  some  questions  but  raises  others.  One  of  the 
areas  that  could  receive  further  attention  is  the  selection  of  the  Lyapunov  controller  gains. 
The  theory  requires  positive  definite  matrices  to  ensure  stability  but  offers  no  insights  con¬ 
cerning  selection  of  the  matrices  to  improve  performance.  For  any  given  set  of  controller 
matrices,  one  expects  the  relative  merits  of  the  point  controller,  tracking  controller,  and 
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modified  tracking  controller  to  remain  the  same  1  lowever,  still  better  performance  might 
be  achieved  across  the  board  if  the  gains  were  optimized. 


Rather  than  merely  changing  Lyapunov  controller  gains,  one  might  investigate  another 
Lyapunov  controller  by  beginning  with  a  different  candidate  Lyapunov  function  than  the 
one  presented  here  The  choices  are  infinite  and  the  results  and  performance  difficult  to 
predict. 

Trajectory  optimization  is  another  area  that  would  benefit  from  further  work.  The 
function  minimization  algorithm  used  to  select  polynomial  coefficients  converged  to  local 
minima  solutions  depending  on  the  initial  guess  for  the  coefficients.  The  search  for  a  glo¬ 
bal  minimum  for  a  particular  order  polynomial  requires  further  investigation.  An  alternate 
approach  with  respect  to  trajectory  optimization  is  to  use  some  function  other  than  a  sim¬ 
ple  polynomial  to  describe  the  trajectory.  Possible  trajectories  might  be  Tchebycheff  poly¬ 
nomials,  Legendre  polynomials,  or  Fourier  series. 

To  help  bridge  the  gap  between  the  analytical  model  and  the  real  world  hardware,  one 
could  consider  modifying  the  controller  to  include  joint  friction,  actuator  backlash,  sensor 
noise,  and  flexibility.  One  could  also  consider  using  a  minimum  generalized  coordinate 
formulation.  One  might  also  attack  the  differences  from  the  hardware  perspective  by 
seeking  components  that  more  closely  resemble  those  in  the  analytical  model.  Another 
improvement  in  the  experiment  would  be  to  replace  the  existing  joint  velocity  approxima¬ 
tions  with  either  an  observer  or  an  actually  velocity  measurement. 

Finally,  it’s  a  three  dimensional  world.  Extending  the  analytical  model  and,  if  possi¬ 
ble,  the  laboratory  experiment  to  include  out  of  plane  motion  should  be  considered. 
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APPENDIX  A:  EXPERIMENTAL  CONTROL  BLOCK 

DIAGRAMS 

This  appendix  includes  the  block  diagrams  of  the  System  Build  super  blocks  made  to 
control  the  SRS.  The  heirarchy  among  the  super  blocks  is  illustrated  in  Figure  59.  Both  is 
the  parent  superblock.  The  ohters  are  lower  level  super  blocks 


Both  - 

Parameters 

I 

Trajectories 

Encoders 

LeftAngles - 

Parti 

Controller  : 

Part2 

Part3 

Figure  59:  Super  Blocks  Hierarchy 

The  block  diagram  for  super  block  Both  is  shown  in  Figure  60.  Inputs  into  the  dia¬ 
gram  include  the  sensor  signals  from  the  hardware  and  user  operated  dials  to  select  the 
controller  gains  and  enable  switches  which  select  the  combination  of  joint  actuators  to 
enable  The  outputs  include  commanded,  reference,  and  error  signals  for  each  of  the  cen- 
terbody  angle,  joint  angles,  and  payload  angle.  Motor  current  commands  to  the  Kepco 
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power  supplies  are  also  outputs.  Block  56  contains  the  system  parameter  values  for  the 
experimental  hardware.  This  block  is  expanded  and  displayed  in  Figure  61.  Block  8  con¬ 
tains  the  position  and  velocity  values  for  a  reference  trajectory  in  a  look-up  table.  It  also 
contains  a  table  to  reset  the  system  back  to  its  original  geometry  to  permit  rerunning  the 
reference  trai^'ory.  This  block  is  expanded  in  Figure  62  Conversion  of  the  encoder 
pulses  from  the  right  manipulator  into  angle  and  angle  rate  information  is  done  in  Block  7 
which  is  expanded  in  Figure  64.  Conversion  of  the  encoder  pulses  from  the  right  manipu¬ 
lator  into  angle  information  for  the  left  manipulator  is  done  in  Block  49.  Details  of  this 
block  are  shown  in  Figures  64-64.  The  PD  controllers  which  convert  the  error  signals  into 
actuator  commands  are  in  Block  40.  This  block  is  expanded  in  two  parts.  The  actuator 
commands  for  the  right  and  left  manipulator  are  shown  in  Figures  68  and  69  respectively. 
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Discrete  Super-Block  Stapling  Interval  First  Saaple  Ext. Inputs  Ext. Outputs  Enable 

both  0.0100  0.  48  25  Parent 
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CM 


Figure  62:  Reference  Trajectory  Block  Diagram 
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eta  Super-Block  Swelling  Interval  First  Sajapla  Ext. Inputs  Ext. Outputs  Ensbls 

Parti  0.0100  0.  11  14  Parent 


isczete  Super-Block  Settling  Interval  First  Seattle  Ext. Inputs  Ext. Outputs  Enable 
Par t3  0.0100  0.  19  3  Parent 


APPENDIX  B:  MATLAB  CODE 


The  following  listings  are  the  MATLAB  code  used  for  the  analytical  simulations.  The 
modules  are  included  in  alphabetical  order.  The  hierarchical  relationship  between  the 
modules  is  illustrated  in  Figure  70.  The  integration  modules  ode2  and  odemin  are  minor 
variants  of  the  MATLAB  supplied  module  ode45  The  modifications  permit  more  param¬ 
eters  to  be  passed  to  and  from  these  modules  without  having  to  include  the  extra  variables 
in  the  state  vector  Similarly,  fminu2  modified  the  MATLAB  unconstrained  function  min¬ 
imization  module,  fminu,  to  include  some  diagnostic  statements  while  running. 


MainOpt  — 

fminu2 - 

MainMin 

~1 

odemin — 

Main2 - 

RefMin2 

ode2 

1 

- 1  AngMo2 

1 

Draw3 

Eqn2 

AngMo 

Ref2 

Matx 

Matx 

Matx 

MatxFix 

MatxFix 

MatxFix 

AngMo2 

AngMo2 

Figure  70:  MATLAB  Modules  Hierarchy 
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A.  AngMo 


%  Filename  is  "AngMo.m" 

%  This  file  calculates  the  angular  momentum  of  the  system 
function  (Us}  =  AngMo(Ls,Ms,CMs,Is,Q,Qdot) 

%  OUTPUT. 

%  I  Is  =  1  \7  row  vector  of  angular  velocities.  The  first  element  is  for 
%  the  centerbodv.  The  next  lour  elements  are  tor  the  left  upper 

%  and  lower  arm  followed  by  the  nght  upper  and  lower  arm  The 

%  last  two  elements  are  for  the  pavload  and  a  total  of  all  the 

%  previous  elements.  (110  UL 1  1IL2I1R1  I1R2  I  IP  1 1  Total  | 

% 

%  INPUT: 

%  Ls  =  7x  1  vector  of  lengths  (m) 

%  1st  element  =  distance  from  origin  to  left  arm  mount 
%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  toward  wrist) 

%  4th  element  =  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wnst  toward  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  [L0;  LI;  L2;  LP;  R2;  R1 ;  R0] 

%  Ms  =  6x  l  column  vector  containing  the  masses  (kg ) 

%  1  st  element  =  mass  of  spacecraft  centerbodv 

%  2nd  &  3rd  elements  =  mass  of  left  arm  (upper  arm  then  lower  arm) 
%  4th  &  5th  elements  =  mass  of  right  arm  (upper  arm  then  lower  arm ) 
%  6th  element  =  payload  mass 
%  [MO;  ML  1 ;  ML2;  MR  1 ;  MR2;  MP| 

%  CMs  =  6x  1  column  vector  containing  center  of  mass  locations 
%  [LcO;  LcL  1;  LcL.2;  LcRl;  LcR2;  LcP| 

%  Is  =  6.\  I  column  vector  containing  the  moments  of  inertias  about  the 
%  respective  body's  center  of  mass  (kg  mA2) 

%  1  st  element  =  inertia  of  spacecraft  centerbodv 

%  2nd  &  3rd  elements  =  inertia  of  left  arm  (upper  aim  then  lower  arm ) 
%  4th  &  5th  elements  =  inertia  of  right  arm  (upper  arm  then  lower  arm) 
%  6th  element  =  payload  inertia 
%  [10;  ILL.  IL2;  IR1;  IR2;  IP] 

%  Q  =  8x1  column  vector  of  generalized  coordinates 
%  Qdot  =  8x1  vector  of  generalized  velocities 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CON  VERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

L()  =  Ls(l); 

LI  =  Ls(2); 

1.2  =  Ls(3); 

LP  =  Ls(4); 

R2  =  Ls(5); 

R1  =  Ls(6); 

R0  =  Ls(7); 

%  Member  masses  (kg) 

MO  =  Ms(l); 

ML1  =  Ms(2); 

ML2  =  Ms(3); 

MR  1  =  Ms(4); 

MR2  =  Ms{5); 

MP  =  Ms(6); 

%  Center  of  mass  distances  (m) 
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LoO  =  CMs(l); 

I. cLl  =  CMs(2); 

1x1,2  =  CMs(3); 

I  cRl  =  CMs(4); 

IxR2  =  CMs(5); 

IxP  =CMs(6);  %measured  from  left  end 

%  MOI  about  center  of  mass 
10  =  ls(l); 

II,  I  =  Is(2); 

11.2  =  ls(3); 

1R1  =  ls(4); 

IR2  =  Is(5); 

IP  =  ls(6); 

%  Coordinates  (rad  &  m) 

ThO  =Q(l); 

Thl,  1  =  Q(2); 

ThL2  =  Q(3); 

ThRl  =  Q(4); 

ThR2  =  Q(5); 

ThP  =  Q(6), 

XP  =  Q(7); 

YP  =  Q(8); 

%  Coordinate  Rates  (rad/sec  &  m/sec) 

ThOd  =  Qdot(  I ); 

ThLld  =  Qdot(2); 

ThL2d  =  Qdot(3); 

ThRld  =  Qdot(4); 

ThR2d  =  Qdot(5); 

ThPd  =Qdot(6); 

XPd  =  Qdot(7); 

YPd  =  Qdot(8); 

%  Angular  Momentum 
HO  =  Th0d*(10  +  M0*Lc0A2); 

HL 1  =  Th0d*(lL  1 +ML 1  *(L0A2+LcL  1  A2+2*LO*Lcl,  1  *cos(ThI.  1 )))  +  .. . 

ThL  ld*(IL  1 +ML 1  *(LcL  1  A2+L0*LcL  1  *cos(ThL  1 ))). 

HL2  =  Th0d*(IL2+MI.2*([,0A2+I,lA2+LcL2A2+2*I.O*I.l  *cos(Thl.l)  +  .. 
2*L  1  *LcL2*cos(ThL2)+2*L0*LcL2*cos(  Till .  I  +Thl,2)))  +  ... 
ThL  ld*(IL2+MI,2*(LlA2+LcL2A2+L0*!,l*cos(  Till,  1)  +  ... 

2*L  1  *LcL2*cos(ThL2)+L0*LcL  2*cosrrhI .  1  +  Ihl  ,2)))  +  . . 
ThL2d*(IL2+ML2*(LcL2A2+Ll*LcL2*cos(  I'hL2)  + ... 
L0*LcL2*cos(ThL  1 +ThL2))); 

I IR 1  =  Th0d*(IR  1 +MR 1  *(R0A2+LcR  1  A2+2*RO*I.cR  1  *cos(ThR  I )))  +  ... 

ThR  1  d*(IR  1 +MR 1  *(LcR  1  A2+R0*LcR  1  *cos(ThR  1 ))). 

HR2  =  ThOd*(IR2+MR2*(ROA2+RIA2+LcR2A2+2*RO*R  I  *cos(ThRI )  + 
2*R  1  *LcR2*cos(ThR2)+2*R0*LcR2*cos(  ThR  1  +ThR2)»  +  ... 
ThR  I  d*(IR2+MR2*(R  1  A2+LcR2A2+RO*R  1  *cos( ThR  1 )  +  . 

2*R  1  *LcR2*cos(ThR2HR0*LcR2*cos(ThR  l  +ThR2)))  +  ... 
ThR2d*(IR2+MR2*(LcR2A2+R  1  *LcR2*cos(ThR2)  +  ... 
R0*LcR2*cos(ThR  1  +ThR2))); 

1  IP  -  ThPd*IP  +  MP*(-XPd*  YP  +  YPd*XP), 

IITotal  =  HO  +  HL  1  +  HL2  +  HR1  +  HR2  +  HP; 

Hs  =  (HO  HL  1  HL2  HR  1  HR2  HP  HTotal]; 
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B.  Anpl**o2 


%  Filename  is  “AngMo2  m" 

%  This  file  calculates  the  angular  momentum  of  the  svsiem 
%  Version  2  also  finds  the  rate  of  change  of  angular  momentum 
function  [Hs,  Hdots]  =  AngMo2(Ls,Ms,CMs,ls,Q.0dot.Qddot) 

%  OUTPUT: 

%  Us  =  1\7  row  vector  of  angular  velocities.  The  first  element  is  for 
%  the  centerbody  The  next  four  elements  are  for  the  left  upper 

%  and  lower  arm  followed  by  the  right  upper  and  lower  arm  The 

%  last  two  elements  are  for  the  pavload  and  a  total  of  all  the 

%  previous  elements  (110  HL 1  HL2  1 IR 1  I IR2  1  IP  I  ITotal] 

%  I  Idols  =  1x7  row  vector  of  the  change  in  angular  velocities.  The  order 
%  is  the  same  as  for  1  Is 
% 

%  INPUT. 

%  Ls  =  7x  1  vector  of  lengths  (m) 

%  I  st  element  =  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wit  left  arm  (from  shoulder  toward  wrist) 

%  4th  element  =  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  toward  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  [L0;  L 1 ;  L2,  LP;  R2;  R 1 ;  R0( 

%  Ms  =  6x  1  column  vector  containing  the  masses  <  kg) 

%  I  st  element  =  mass  of  spacecraft  centerbody 

%  2nd  &  3rd  elements  =  mass  of  left  arm  (upper  arm  then  lower  arm) 
%  4th  &  5th  elements  =  mass  of  right  arm  (upper  arm  then  lower  arm) 
%  6th  element  =  payload  mass 
%  [MO;  ML  1 ;  ML2;  MR  1 :  MR2;  MP] 

%  CMs  =  6x1  column  vector  containing  center  ot  mass  locations 
%  [LcG,  LcL  1 ;  LcL2;  LcR  1 ;  LcR2;  LcP( 

%  Is  =  6x1  column  vector  containing  the  moments  of  inertias  about  the 
%  respective  body's  center  of  mass  (kg  mA2) 

%  1  st  element  =  inertia  of  spacecraft  centerbody 

%  2nd  &  3rd  elements  =  inertia  of  left  arm  (upper  arm  then  lower  arm) 
%  4th  &  5th  elements  =  inertia  of  right  arm  (upper  arm  then  lower  arm) 
%  6th  element  =  payload  inertia 
%  [10;  ILL,  IL2;  1R1.  IR2;  IP] 

%  Q  =  8x1  column  vector  of  generalized  coordinates 
%  Qdot  =  8x  1  vector  of  generalized  velocities 
%  Qddot  =  8x  I  vector  of  generalized  accelerations 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

L0  =  Ls(l); 

LI  =  Ls(2); 

L2  =  Ls(3); 

LP  =  Ls(4); 

R2  =  Ls(5); 

R1  =Ls(6); 

R0  =  Ls(7); 

%  Member  masses  (kg) 

MO  =  Ms(l); 

ML1  =  Ms(2); 

ML2  =  Ms(3); 

MR1  =  Ms(4); 
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MR2  =  Ms(5); 

MP  =  Ms(6); 

%  Center  of  mass  dislances  (m) 

I. cO  =  CMs(l). 

Id.  I  =  CMs(2); 

1x1.2=  CMs(3); 
ixRl  =  CMs(4); 
ixR2  =  CMs(5); 

IxP  =  CMs(6),  %measured  from  left  end 

%  MOl  about  center  of  mass 
10  =Is(l); 

II.  I  =  Is(2); 

II. 2  =  Is(3); 

IR1  =  lsv4); 

IR2  =  ls(5); 

IP  =  ls(6); 

%  Coordinates  (rad  &  m) 

ThO  =Q(1); 

ThL  I  =  Q(2); 

ThL  2  =  Q(3); 

ThR  I  =Q(4); 

ThR2  =  Q(5); 

ThP  =  Q(6); 

XP  =Q(7); 

YP  =  Q(8); 

%  Coordinate  Rates  (rad/sec  &  m/sec) 

ThOd  =  Qdot(  I ); 

ThL  Id  =  Qdol(2); 

ThL2d  =  Qdot(3); 

ThR  Id  =  Qdot(4); 

ThR2d  =  Qdot(5); 

ThPd  =  Qdot(6); 

XPd  =  Qdot(7); 

YPd  =  Qdot(8); 

%  Coordinate  Accelerations  (rad/sec A2  &  m/sec A2) 

ThOdd  =  Qddot(l); 

ThL  1  dd  =  Qddot(2); 

ThL2dd  =  Qddot(3); 

IhRIdd  =  Qddot(4); 

ThR2dd  =  Qddot(5); 

ThPdd  =  Qddot(6); 

XPdd  =  Qddot(7); 

YPdd  =Qddot(8); 

%  Angular  Momentum 
HO  =  Th0d*(I0  +  MO*LcOA2); 

HL 1  =  ThOd*(IL  1 +ML 1  *(L0A2+LcL  1  A2+2*L0*l.d .  1  *cos(ThI.  1 )))  +  . .. 

ThL  1  d*(IL  1 +ML 1  *(LcL  1  A2+L0*LcL  1  *cos(ThL  l ))); 

HL2  =  Th0d*(IL2+ML2*(L0A2+L  1  A2+LcL2A2+2*I.O*L  1  *cos(ThL  1 )  + 
2*Ll*LcL2*cos(ThL2)+2*L0*LcL2*cos(ThLl+ThL2)))  + ... 
ThLld*(IL2+ML2*(L  1  A2+LcL2A2+LO*L  I  *cos(ThL  I )  +  ... 
2*Ll*LcL2*cos(ThL2)+LO*LcL2*cos(  Hil.  I+Thl.2)))  + 
ThL2d*(IL2+ML2*(LcL2A2+L  1  *LcL2*cos(  l'hl.2)  +  ... 
L0*LcL2*cos(ThL  l+ThL2))); 

HR1  =  ThOd*(IRl+MRl*(ROA2+LcRlA2+2*RO*LcRl*cos(ThRl))>  +  . 
ThRld*(lRl  +MR1  *(LcR  1  A2+R0*LcRl  *cos(ThR  1»); 
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I IR2  =  ThOd*(IR2+MR2*(ROA2+RlA2+LcR2A2+2*RO*Rl*cos(ThRl )  + 
2*Rl*LcR2*cos(ThR2)+2*RO*LcR2*cos<  ThR l+ThR2)))  + 
ThRld*(IR2+MR2*(R !  A2+l.eR2A2+RO*R !  *eos(  IhR  I)  + 
2*Rl*LcR2*cos(ThR2)+RO*LcR2*cos<ThR  l+ThR2)))  + 
ThR2d*(IR2+MR2*(LcR2A2+Rl*LcR2*cos<  ThR2)  +  ... 
R0*LcR2*cos(ThR  1  +ThR2))); 

HP  =  ThPd*IP  +  MP*(-XPd*YP  +  YPd*XP). 
i  ITotal  =  HO  +  HL I + I IL2  +  HR  1  +  HR2  +  HP. 

I  Is  =  |H0  HL1  HL2HRI  I IR2  IIP  HTotalJ; 


"'ll  Change  in  angular  momentum 
I  lOd  =  ThOdd*(IO  +  M0*Lc0A2); 

I II.  Id  =  ThOdd*(lL l+MLl *(L0A2+LcL  1  A2+2*LO*l .cl.  1  *cos<  ThL  1)))  +  ... 

ThL !dd*(Il.  1+ML I *(I.cL  1  A2+L0*l.cl.  I *cos(  l  hI.  10)  - ... 

ThOd*ThI.  ld*2*MI.  1  *I.0*LcL  I  *sin( Thf .  I )  - 
ThL  ldA2*ML  1  *I.O*l.cL  1  *sin(Thl.  I ). 

I II. 2d  =  ThOdd*(IL2+ML2*(L()A2+LlA2+LcI.2  '2+2*l  . 0*1.1  *cos(  I  hi.  I )  + 

2*L  1  *LcL2*cos(ThL2)+2*L0*LcL2*cos(ThI.  1  +ThI.2»)  +  ... 

ThL  ldd*(IL2+ML2*(L  I  A2+LcL2A2+L.0*I.  I  *cos(ThL  1 )  +  ... 

2*L  I  *LcL2*cos(ThL2)+LO*LcL2*cos(ThL  1  +ThL2)»  +  .  . 
ThL2dd*(IL2+ML2*(LcL2A2+L  1  *l.el  .2*cos(  ThI  .2)  +  ... 

L0*LcL2*cos(ThL  1  +ThL2))>  - ... 

ThOd*ThL  ld*2*ML2*(L0*L  1  *sin(Thl. I )+L()*LcI.2*sin(ThI.  l  +  lhl.2))  - ... 
ThOd*ThL2d*2*ML2*(Ll*LcL2*sin(ThI.2)+I.O*I.cL2*sin(  lhI.l+ThL2))  -... 
ThL  ld*ThL2d*2*ML2*(L  1  *LcL2*sin(ThL2)+I.0*I.cL2*sin(  ThL  1  +ThL2)>... 
ThLldA2*ML2*(LO*Ll*sin(ThLl)+L.O*l.cI.2*sin(ThLl+ThI.2))  - ... 
ThL2dA2*ML2*(1. 1  *LcL2*sin(ThL2)+!  .0*LcI.2*sin(ThL  1  +ThL2)). 

HR  Id  =  ThOdd*(IR  I+MRI  *(R0A2+LcR  1  A2+2*RO*LcR  I  *cos(ThR  1 )))  +  ... 

ThR  1  dd*( IR 1 +MR 1  *(LcR  1  A2+R()*I ,cR  1  *cos(  IhR  1 )))  - ... 

ThOd*ThR  1  d*2*MR  1  *RO*LcR  1  *sm(  ThR  1)  -  ... 

ThR  IdA2*MR  1  *RO*LcR  1  *sin(ThR  1 ). 

HR2d  =  ThOdd*(IR2+MR2*(ROA2+R  1  A2+LcR2A2+2*R0*R  I  *cos(ThR  1 )  +  ... 
2*Rl*LcR2*cos(ThR2)+2*R0*LcR2*cos(ThRl+ThR2»)  + 
ThRIdd*(IR2+MR2*(Rl  A2+LcR2A2+RO*R  1  *cos(ThR  1 )  +  ... 

2*R  1  *I.cR2*co.s(ThR2)+R0*LcR2*cos(ThR  1  +ThR2»)  +  ... 
ThR2dd*([R2+MR2*(LcR2A2+Rl*LcR2*cos(  ThR2)  + ... 

R0*LcR2*cos(ThR  1  +ThR2)))  -  ... 

ThOd*ThR  1  d*2*MR2*(R0*R  1  *sin(ThR  1  )+R0*LcR2*sin(ThR  1  +ThR2))  - . .. 
ThOd*ThR2d*2*MR2*(R  1  *LcR2*sintThR2)+R0*LcR2*sin(  I  hR  1  +ThR2))-.. . 
ThR  1  d*ThR2d*2*MR2*(R  1  *LcR2*sin(ThR2)+RO*LcR2*sm<  ThR  1  +ThR2))-... 
ThRIdA2*MR2*(RO*Rl*sin(ThRl)+RO*LcR2*sin(ThRI+ThR2))  - ... 
ThR2dA2*MR2*(R  1  *LcR2*sin(ThR2)+RO*I  ,cR2*sin(ThR  I  +ThR2)); 

I  IPd  =  ThPdd*IP  +  MP*(-XPdd*YP  -  XPd*YPd  +  YPdd*XP  +  YPd*XPd)i 
HdTotal  =  HOd  +  HL  Id  +  HL2d  +  HRId  +  HR2d  +  HPd; 

Hdots  =  [HOd  HLld  HL2d  HRId  HR2d  HPd  HdTotal]. 


C.  Draw3 

%  Filename  is  ’Draw3.m' 


function[X,Y]  =  Draw3(Lengths,AngConst,AngHist.Interval) 


%  This  file  draws  the  dual  arm  spacecraft  stick  figure 

% 

%  INPUTS: 

% 

%  Lengths  =  7x1  vector  of  link  lengths  (m) 
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%  1st  element  is  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wrt  left  arm  ( from  shoulder  toward  wrist) 

%  4th  element  is  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  toward  shoulder) 

%  7th  element  is  distance  from  right  arm  mount  to  origin 

%  AngConst  =  vector  of  angles  to  arm  mounting  locations  wrt  centerbodv  coord 
%  frame  (angle  for  left  arm  then  angle  tor  right  arm ) 

%  AngHist  =  ny.6  matrix  of  angle  histones  Each  row  represents  a 
%  specific  time.  Each  column  represents  a  specific  joint 

%  angle  (except  the  payload  angle  is  inertial) 

%  1st  column  is  the  center  body  angle 

%  2nd  &  3rd  columns  are  the  left  arm  shoulder  and  elbow 

%  4th  &  5th  columns  are  the  right  arm  shoulder  and  elbow 

%  6th  column  is  the  payload  (this  angle  is  inertial) 

%  Interval  =  plot  every  "interval'th"  time 
% 

% 

%  OUTPUTS: 

% 

%  X  =  vector  history  of  joint  x  coordinates 
%  Y  =  vector  history  of  joint  y  coordinates 

%  X  &  Y  treat  the  system  as  a  closed  chain  beginning  at  the  centerbodv  origin, 
%  outward  along  the  left  arm,  across  the  payload,  inward  along  the  right  arm, 

%  and  back  to  the  origin. 


[Times.dummy]  =  size(  AngHist); 

Links  =  length(Lengths), 

X(l,l)  =  0; 

Y(l.l)  =  0; 

%  Convert  the  joint  angles  to  inertial  angles  and  reorder  them  for  closed  chain  use 
NAng(:,l)  =  AngHistf:,  1)  +  AngConst(l)*ones(  Times.  1 ); 

NAng(:,2)  =  NAng(:,l)  +  AngHist(:,2); 

NAng(:,3)  =  NAng(:,2)  +  AngHist(.,3), 

NAng(:,4)  =  AngHist(:,6); 

NAng(:,7)  =  AngHistf:,  1 )  +  AngConst(2)*ones(Times,  1 )  +  pi; 

NAng(:,6)  =  NAng(:,7)  +  AngHist(:,4); 

NAng(:,5)  =  NAng(:,6)  +  AngHist(.,5); 

P=  1; 

while  p  <=  Times 
for  q=  1  Links 
Lastx  =  0; 

I.asty  =  0; 
forr  =  l:q 

Lastx  =  Lastx  +  Lengths(r)*cos(NAng(p,r)); 

Lasty  =  Lasty  +  Lengths(r)*sin(NAng(p,r)); 
end 

X(q+l,p)  =  Lastx, 

Y(q+l,p)  =  Lasty; 
end 

p  =  p  +  Interval; 
end 

X  =  [X(  1  ;Links,:);  X(2,:);  X(Links,:);  X(Links+l,:)j; 

Y  =  [Y(l:Links„);  Y(2,:);  Y(Links,;);  Y(Links+l,:)J; 

%  Plot  the  Final  Case 
for  q  =  1  :L  inks 
Lastx  =  0; 

Lasty  =  0; 
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lor  r  =  I  :q 

l.astx  =  Lasix  +  l.englhs(r)*cos(NAngl Times.r)), 

Lasly  -  Laslv  +  I.engths(r)*sin(NAng(Times.r)); 
end 

XFinal(q+l ,  1 )  =  Lasix; 

YFinal(q+l,l)=  Lastv. 
end 

XFinal  =  [XFinal(l:Links,:);  XFinal(2,:);  XFinalt  Links,:).  XFinal(Links+i,:)|; 
YFinal  =  [ YFinal(  1  Links.:);  YFinal(2,:);  YFinaKLinks.:);  YFinal(Links+l,:)j, 

elg; 

axisf square') 

plol(X,Y,'-\XFinaI.YFinal.'-',XFinal,YFinal.Y,  X(:.i  ).Y(:.l),'o'); 

\label(’X  (m)’);ylabel(’Y  (m)'); 
axis( 'normal') 


D.  Eqn2 


%  Filename  is  'Eqn2.m' 

%  DilTerential  Equations  for  numerical  integrator 
function  (Xdot,U,TorqRef.Aqdot,J,Res,LHS,RHS,Delq|  =... 

Eqn2(T,X,Ls,Ms,CMs,ls,BoundC,Gains.XfDes.Wu.Wc,Coef.ConstMat) 

%  OUTPUT: 

%  xdot  =  derivatives  of  state  vector  at  time  T 

%  U  =  column  vector  of  actual  torques  commanded  at  time  T  arranged 
%  as  [U 1 ;  U2;  U6;  U5|  where  the  number  denotes  the  joint 
%  associated  with  that  torque 

%  TorqRef  =  column  vector  of  reference  torques  that  should  be  applied 
%  at  time  T  if  the  motion  followed  the  reference  maneuver  exactly 
%  These  are  arranged  in  the  same  order  as  U. 

%  Res  =  column  vector  of  residuals  after  EOM  are  evaluated  with  the 
%  calculated  reference  torques.  (Residuals  should  be  zero). 

%  Aqdot  =  column  vector  of  A*qdot.  This  is  a  test  to  see  if  the 
%  constraint  equation  (A*qdot  =  0)  is  satisfied 

%  LHS  =  column  vector  of  the  EOM  left  hand  side  (LI  IS  =  M*qddol  +  GTilda) 
%  RHS  =  column  vector  of  the  EOM  right  hand  side  (RI  IS  =  BTilda*u) 

% 

%  INPUTS: 

%  T  =  time  (sec) 

%  State  Vector,  X,  is  defined  as  follows: 

%  XI  =  Theta  0  (rad) 

%  X2  =  Theta  L 1  (rad) 

%  X3  =  Theta  L2  (rad) 

%  X4  =  Theta  Rl  (rad) 

%  X5  =  Theta  R2  (rad) 

%  X6  =  Theta  P  (rad) 

%  X7  =  X  component  of  Payload  Center  of  mass  position  (m) 

%  X8  =  Y  component  of  Payload  Center  of  mass  position  (m) 

%  X9  =  Theta  0  Dot  (rad/ sec) 

%  X10  =  Theta  L 1  Dot  (rad/sec) 

%  XI 1  =  Theta  L2  Dot  (rad/sec) 

%  X 1 2  =  Theta  R 1  Dot  (rad/sec) 

%  X 1 3  =  Theta  R2  Dot  (rad/sec) 

%  X 1 4  =  Theta  P  Dot  (rad/sec) 

%  X 1 5  =  X  component  of  Payload  Center  of  mass  velocity  (m/sec) 

%  XI6  =  Y  component  of  Payload  Center  of  mass  velocity  (m/sec) 

%  XI 7  =  integral  of  the  absolute  value  of  the  centerbody  disturbance  torque 

%  XI 8  =  integral  of  the  centerbody  disturbance  torque  squared 
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%  I  ,s  =  7x  1  vector  of  lengths  (m ) 

%  1  st  element  =  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  toward  wrist) 

%  4th  element  =  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  toward  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  |L0;  L 1 ;  L2;  LP;  R2;  R 1 ;  R0| 

%  Ms  =  6x  1  column  vector  containing  the  masses  <  kg ) 

%  I  st  element  =  mass  of  spacecraft  centet  hodv 
%  2nd  &  3rd  elements  =  mass  of  lelt  arm  (upper  arm  then  lower  arm ) 
%  4th  &  5th  elements  =  mass  of  nght  arm  ( upper  arm  then  lower  atm ) 
%  6th  element  =  pavload  mass 
%  |  MO.  ML  1 ;  ML 2;  MR  1 ;  MR2;  MP] 

%  CMs  =  6\  1  column  vector  containing  center  of  mass  locations 
%  [LcO;  LcL  1 ,  LcL2;  LcR  1 .  LcR2;  LcP  | 

%  Is  =  6x  1  column  vector  containing  the  moments  of  inertias  about  the 
%  respective  body's  center  of  mass  (kg  mA2) 

%  I  st  element  =  inertia  of  spacecraft  cenlerbody 

%  2nd  &  3rd  elements  =  inertia  of  left  arm  ( upper  arm  then  lower  arm ) 
%  4th  &  5th  elements  =  inertia  of  right  arm  ( upper  arm  then  lower  arm ) 
%  6th  element  =  payload  inertia 
%  1 10;  11,1;  IL2;  IR1;  IR2;  IP) 

%  BoundC  =  boundrv  conditions  for  the  problem  The  first  column 
%  contains  the  initial  x  and  v  component  of  points  Q  &  P 
%  respectively,  the  x  component  of  the  nght  arm  base,  the 
%  problem  start  time,  and  the  simulation  stop  time  The  second 
%  column  contains  the  x  and  y  component  of  points  Q  &  P 
%  respectively,  the  x  component  of  the  right  arm  base,  the 
%  stop  time  for  the  ideal  reference  maneuver,  and  a  Hag  to 
%  activate  or  deactivate  the  controller.  The  origin  for  the 
%  x  and  y  components  is  the  base  of  the  left  arm. 

%  Wu  =  6x6  control  torque  cost  weighting  matrix 
%  Wc  =  8x8  constraint  cost  weighting  matrix 
%  Gains®  1x2  column  vector  of  controller  gains.  The  first  value  is 
%  for  position  gams  and  the  second  value  is  for  velocity 
%  gains. 

%  XfDes  =  column  vector  containing  desired  values  for  the  angles  at 
%  the  conclusion  of  the  maneuver.  These  are  the  same  angles 

%  the  reference  maneuver  is  trying  to  create.  Thev  are  arranged 

%  as  (ThOf;  ThL  1  f;  ThL2f;  ThR  1  f;  ThR2f;  ThPf|. 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

ThO  =  X(l); 

ThL  1  =  X(2); 

ThL2  =  X(3); 

ThRl  =X(4); 

ThR2  =  X(5); 

ThP  =  X(6); 

Xc  =  X(7); 

Yc  =  X(8); 

ThOd  =  X(9); 

ThL  Id  =  X(  10); 

ThL2d  =  X(  1 1 ); 

ThR  Id  =  X(  1 2); 

ThR2d  =  X(  1 3); 

ThPd  =  X(  1 4); 

Xcd  =  X(  1 5); 

Ycd  =X(16); 
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%  Arms  mount  locations  wrt  spacecraft  centerhodv  coordinate  f  rame  I  rad ) 
ThLO  =  BoundC(5,l);  ThRO  =  BoundC(5,2); 

%  Stop  Times 

1 TR  =  BoundC(6,2),  %  Reference  Torque  stop  time  ( sec) 

I TC  =  BoundC(7, 1 );  %  Controller  stop  lime  I  sec  i 

%  Controller  Hag 
ContFlag  =  BoundC(7,2), 

%  Constraints  Matrix  Flag 
AMatFlag  =  BoundC(8,l); 

%  Centerbody  Reaction  Wheel  Flag 
WlieelFlag  =  BoundC(8,2); 

%  Kinetic  Energy  Test  Flag 
KEFlag  =  BoundCf  11,1); 

%  Inverse  Kinematics  Bypass  Flag 
BvPass  =  BoundCf  1 1 ,2); 

%  Torque  selection  if  bypass  is  enabled 
TorqFlag  =  BoundCf  1 2, 1 ); 

%  Maximum  torque  from  reaction  wheel 
TorqCap  =  BoundC(13.1);  %  Limit  enabled 

TorqMax  =  BoundC(13,2);  %  Limit  amount 

%  Controller  Gains: 

Gpos  =  Gains(l); 

Gvel  =  Gains(2); 


%%%%%%%%%%%%%% 

%%  CALCULATIONS  %% 

%%%%%%%%%%%%%% 

%  EOM:  M*qddot  +  dV/dq  +  G  =  Qf  +  A'*I,am 
%  M  is  mass  matrix 

%  qddot  is  column  vector  of  generalized  coordinate  accelerations 
%  dV/dq  is  the  partial  derivative  of  the  potential  function  with 
%  respect  to  the  generalized  coordinates.  This  term  is  zero  for 
%  this  problem  because  all  motion  is  in  the  horizontal  plane  ( there 
%  is  no  change  in  potential  energy  caused  by  the  motion) 

%  G  is  a  column  vector  which  is  a  function  of  q  and  qdot 
%  Qf  are  generalized  forces  caused  by  joint  torques 
%  A'  is  transpose  of  constraints  matrix 
%  Lam  are  Lagrange  multipliers 


%%%%%%%%%%%%%%%%%% 

%%  State  Vector  &  Derivative  %% 
%%%%%%%%%%%%%%%%%% 

Q  =  [ThO;  ThL  1 ;  ThL2;  ThRl:  ThR2;  ThP.  Xc.  Ye); 

Qdot  =  [ThOd;  ThL  Id;  ThL2d;  ThRld;  ThR2d;  ThPd;  Xcd;  Ycd]; 


%%%%%%%%% 

%%  Matrices  %% 

%%%%%%%%% 

AngConst  =  [ThLO;  ThRO]; 
if  AMatFlag 

[M,G,A,Adot,B]  =  Mat\Fix(Ls,Ms,CMs,Is,Q,Qdot,AngConst); 

else 


US 


|M,G,A,Adot,B|  =  MatxfLs,Ms,CMs,ls,Q,Qdot.AngConst ); 


end 
if  WheelFlag 

H7  =  [  1 . 0;  0, 0.  0.  0,0.0], 

B  =  [B7  B], 
end 

il  ByPass  %  If  true,  then  bypass  calculating  torques  using  inverse 
%  kinematics.  This  branch  of  logic  is  a  v  erification  test 
%  during  program  development  and  is  not  intended  for  regular 
%  use  once  the  program  is  checked  out. 
if  TorqFlag  ==  0 

l  J  =  zeros(6, 1 );  .1  =  0; 

else 

if  TorqFlag  =  I 

U  =  (-0.01;  0;  0;  0;  0;  0|;  J  =  0; 

else 

U  =  [-0.01;  0;  0;  0.01;  0;  0];  J  =  0; 
end 
end 

if  WheelFlag 
U  =  [0;  U(; 
end 

else  %  Normal  program  How  to  find  control  torques 
%%%%%%%%% 

%%  Torques  %% 

%%%%%%%%% 

if  T  <=  TfR,  %  Get  the  appropriate  torque  and  angle  values 
%  from  the  reference  maneuver  calculations 
[TorqRef,  QRef,  QdotRef,  Aqdot,  J,  C 1  Ref,  C2Ref,  C3Ref]  = 

Ref2(Ls,Ms,CMs,Is,BoundC,T,Wu,Wc,Coef,ConstMat); 
else  %  Simulation  is  longer  than  ideal  reference  maneuver 
%  Use  no  reference  torques 
%  Use  the  desired  final  values  as  references 
TorqRef  =  [0,  0;  0;  0;  0;  0]; 

QReff  I )  =  XfDesf  I ); 

QRef(2)  =  XfDes(2); 

QRef(3)  =  XfDes(3), 

QRef(4)  =  XfDes(4), 

QRef(5)  =  XfDes(5); 

QRef(6)  =  XfDes(6); 

QRef(7)  =  XfDes(7), 

QRef(8)  =  XfDes(8); 

QdotReffli  =  XfDes(9); 

QdotRef(2)  =  XfDesf  10); 

QdotRef(3)  =  XfDesf  1 1 ); 

QdotRef(4)  =  XfDes(  1 2); 

QdotRef(5)  =  XfDesf  1 3); 

QdotRef(6)  =  XfDesf  14); 

QdotRef(7)  =  XfDesf  15); 

QdotRef(8)  =  XfDesf  16); 
if  WheelFlag 

TorqRef  =  [0;  TorqRef], 
end 

%  Matrices 
if  AMatFlag 

[MRef,GRef,ARef,AdotRef]  =  Mal\Fix(Ls,Ms.CMs,Is,... 

QRef, QdotRef,  AngConst); 

else 
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lMRef,GRcf.ARcf.AdotRef|  =  Matx(l.s.Ms.CMs.Is.gRef.yd«>lRd-.... 

AngConst  i. 
end 

BRef  =  B. 

Pi  I  Rel  =  ARef  *in  v(  ARePin  v(  MRef)*  A  Ref ) ; 

C 1  Ref  =  inv(MRef)*(eye(M)  -  Pt  1  RePARePinvi  MRefi  )*HRet  . 

C2Ref  =  -invl  MRefPPl !  RePAdotRef. 

C3Ref  =  inv(MRef)*(Pt  1  RePARePinvi  MRefi  -  cve<M))*GRef: 
end 

if  ContFlag  %  Controller  is  on 
Delq  =  Q  -  QRef ; 

Delqdot  =  Qdot  -  QdotRef; 

%  Controller  calculations 
Ptl  =  A'*inv(A*inv(M)*A’V, 

Cl  =  inv(M)*(eye(M)  -  Ptl*A*inv(M))*B; 

C2  =  -inv(M)*Ptl  *Adot; 

C3  =  inv(M)*(Ptl*A*inv(M)  -  eye(M))*G. 

F2  =  Gpos  *  Delq; 

F2=  [F2(l;6),0;0|; 

Kv  =  Gvel  *  cye(M); 

Kv(7,7)=0;  Kv(8,8)=0; 

Pt3  =  pinv(C  1); 

%  Pt3  =  inv(Cr*Cl)*Cl';  %  Resulted  in  pooriv  conditioned  mainx 

%%%%%%%%%%%%%%%%%%%% 

%%  Complete  Lyapunov  Controller  %% 
%%%%%%%%%%%%%%%%%%%% 

U  =  Pt3*(-Kv*Delqdot  +  CIRePTorqRef  -  (C2*Qdot  -  C2RePQdolReD  - . 
(C3  -  C3Ref)  -  F2); 

%%%%%%%%%%%%%%%%%%%%%% 

%%  Simplified  Lyapunov  Controller  %% 

%%  (removes  reference  torques  and  %% 

%%  assumes  C2  and  C3  terms  are  small)  %% 
%%%%%%%%%%%%%%%%%%%%%% 

%  Kp  =  Gpos  *  eye(M); 

%  Pt3  =  pmv(ClReO, 

%  U  =  Pt3*(-(Kv+C2Ref)*Delqdot  -  Kp*Dc!q)  +  TorqRef. 

else  %  Controller  is  off 

U  =  TorqRef;  %  Don't  adjust  torques  from  reference  maneuver 
Delq  =  999*ones(8, 1 );  %  Dummy  value  for  trajectory  error 
end  %  End  of  Control  Loop 

if  WheelFlag 
J  =  abs(U(  I )); 
else 
J  =  0; 
end 

end  %  End  of  Kinetic  Energy  Test  Conditional 

if  TorqCap  %  Upper  limit  in  wheel  torque  enabled? 
if  abs(U(l))  >  TorqMax 
if  U(  1 )  >  0 

U(I)  =  TorqMax; 

else 

U(l)  =  -TorqMax; 

end 

end 

end 


%%%%%%% 
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%%  Qf  %% 

%%%%%%% 

%  Qf  =  Q*u  These  are  the  generalized  forces 
Qf  =  B  *  U. 

%%%%%%%%%%%%%%% 

%%  Lagrange  Multipliers  %% 

%%%%%%%%%%%%%%% 

%  MOM  M*qddot  +  dV/dq  +  G  =  Qf  +  A'*l.am 

%  Solving  the  MOM  for  qddot  gives:  qddol  =  inviM  )*(Qf  +  A'*l.am  -  (I) 
%  Differentiating  the  Pfaifian  form  of  the  constraint  equations 
%  results  in  Adot*qdot  +  A*qddot  =  0 

%  Substitution  of  the  expression  for  qddot  into  the  previous  equation 
%  permits  solving  for  Lam: 

%  Lam  =  inv(A*mv(M)*A'l*(A*inv(Ml*(G-Qf>  -  Adot*qdol) 

Lam  =  inv(A*inv(M)*A')*(A*inv(M)*(G-Qn  -  Adot*Qdol). 

%%%%%%%%%%%%%%% 

%%  Putting  it  all  together  %% 

%%%%%%%%%%%%%%% 

Qddot  =  inv(M)  *  (Qf  +  A'*Lam  -  G), 

|IIs,  Hdots]  =  AngMo2(Ls,Ms,CMs,ls,Q,Qdot. Qddot): 

%  Change  in  total  angular  momentum 
1  Id  =  Hdots(7); 

J  =  [J;  Hd|; 

%  Assemble  derivative  of  state  vector  for  integrator 
Xdot  =  (Qdot,  Qddot.  J(  1 ),  (J(  I  ))A2|; 

%%%%%%%%%%%%%%% 

%%  Troubleshooting  Info  %% 

%%%%%%%%%%%%%%% 

Aqdot  =  A*  Qdot, 

LHS  =  M*Qddol  +  G. 

Rl  IS  =  Qf  +  A'*Lam: 

Res  =  LHS  -  RHS. 


E.  fminu2 


function  [x.OPTlONS]  =  fminu2(FUN,x,Oi’TIONS.GRADFUN.Pl,P2.P3.P4,P5,P6,... 
P7,P8,P9,P10) 

%LM1NU  Finds  the  minimum  of  a  function  of  several  variables 
%  X=FMINU('FUN',XO)  starts  at  the  matrix  XO  and  finds  a  minimum  to  the 
%  function  which  is  described  in  FUN  (usually  an  M-file:  FUN  M). 

%  The  function  'FUN'  should  return  a  scalar  function  value:  F=FUN(X) 

% 

%  X=FMINU('FUN',XO, OPTIONS)  allows  a  vector  of  optional  parameters  to 
%  be  defined.  OPTIONS(  1 )  controls  how  much  display  output  is  given,  set 
%  to  1  for  a  tabular  display  of  results,  (default  is  no  display:  0) 

%  OPTIONS(2)  is  a  measure  of  the  precision  required  for  the  values  of 
%  X  at  the  solution.  OPTIONS(3)  is  a  measure  of  the  precision 
%  required  of  the  objective  function  at  the  solution. 

%  For  more  information  tvpe  HELP  FOPTIONS 
% 

%  X=FMINU('FUN’,XO.OPTIONS,'GRADFUN')  enables  a  function'GRADFUN' 
%  to  be  entered  which  returns  the  partial  derivatives  of  the  function, 

%  df/dX,  at  the  point  X:  gf  =  GRADFUN(X). 

% 


%  Hie  default  algorithm  is  the  BFG.S  Quasi -Newton  method  with  a 
%  mixed  quadratic  and  cubic  line  search  procedure 

%  Copyright  (e)  1990  by  the  MathWorks,  Inc 
%  Andy  Grace  7-9-90 


% . Initialization . - . 

XOUT=x(); 
nvars=length(XOU  T), 

cvalstr  =  |  l'l )  N  | , 
if  ~anv(FUN<48) 
cvalstr=|evalstr,  '(x’|, 
for  i=l  margin  -  4 

cvalstr  =  |evalstr,',P',nuin2str(i)|; 
end 

cvalstr  =  [evalstr, 
end 

if  nargin  <  3,  OPriONS=(|,  end 
ifnargin  <  4,  GRADFUN=[);  end 


if  length(GRADFUN) 
cvalstr2  =  (GRADFUN], 
if  ~any(GRADFUN<48) 
evalstr2  =  |evalstr2,  '(x'J; 
for  i=l  margin  -  4 

evalstr2  =  (evalstr2,',I:,,1num2str(i)|; 
end 

evalstr2  =  [evalstr2, 
end 
end 

f  =  cval(evalstr). 
n  =  length(XOUT). 

GRAD=zeros(nvars,  1 ); 

OLDX=XOUT, 

M  ATX=zeros(3, 1 ), 

MATL=(f;0;0]; 

OLDF=f; 

FIRSTF=f; 

lOLDX,OLDF,HESS,OPTIONS)=optint(XOUT,f,OPTIONS); 
CHG  =  le-7*abs(XOUT)+le-7*ones(nvars,l); 

SD  =  zerosfnvars,  1 ), 
diff  =  zeros(nvars,  1 ); 


OPTlONS(10)=2;  %  Iteration  count  (add  1  for  last  evaluation) 
status  =- 1 ; 

while  status  ~=  I 
%  Work  Out  Gradients 
if  ~length(GRAOFUN)  |  OPTIONS(9) 

OLDF=f, 

%  Finite  difference  perturbation  levels 

%  First  check  perturbation  level  is  not  less  than  search  direction, 
f  =  find(10*abs(CHG)>abs(SD)); 

CHG(f)  =  -0. 1  *SD(f); 

%  Ensure  within  user-defined  limits 
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CHG  =  sign/CHG+eps).*min(max/abs/CHG /.OPTIONS/ 16)),OPTIONS(  17)), 
for  gcnt=  1  nvars 

XOUT(gcnt,  1  )=XOUT(gcnl)+C  I  IG(gent »; 

( )PTIONS(  1 0)=OPTIONS(  1 0)+ 1 ; 
disp('While  Loop  Iteralion  in  Progress'), 
dispd'lterations:  ’,num2str(OPTlONS(10))|); 
disp/('Allowable.  ',num2str(OPTIONS(14))j); 
x(:)  =  XOUT;  f  =  eval/evalstr); 

C.RAD(gcnt)=(f-OLDF)/(CHG/gcnl)); 
if  f  <  OLDF 
OLDF=f; 

cis6 

XOUT(gcnt)=XOUT(gcnt)-CHG(gcnt ); 

end 

end 

%  Trv  to  set  difference  to  le-8  for  next  iteration 
CHG  =  le-8 /GRAD; 
f  =  OLDF; 

%  OPTIONS/  10)=OPTIONS(  10)+nvars; 

%  Gradient  check 

ifOFHONS(9)  ==  1 
GRADFD  =  GRAD; 
x(:)=X  '>UT;  GRAD  =  eval(evalstr2); 
graderr(GRADFD,  GRAD,  evalstr2); 

OPTIONS(9)  =  0; 
end 


else 

OPTIONS/ 1 1  )=OPTIONS(  1 1 )+ 1 ; 
x(:)=XOUT;  GRAD  =  eval(evalstr2); 
end 

% - Initialization  of  Search  Direction - 

if  status  ==  -  I 
SD—GRAD; 

FIRSTF=f; 

OLDG=GRAD; 

GDOLD=GRAD'*SD; 

%  For  initial  step-size  guess  assume  the  minimum  is  at  zero. 

OPTIONS/ 18)  =  max/0.01,  min([l,2*abs(17GDOLD)])); 
if  OPTIONS/ 1)>0 

%  disp([sprintf('%5.0f  %  1 2.3g  %12.3g  ’,OPTIONS/10),f,... 

OPTIONS/ 18)),sprintf('%  12. 3 g  ’,GDOLD)|); 
end 

XOUT=XOUT+OPTIONS(  1 8)*SD, 
status=4; 

if  OPTIONS(7)=0;  PCNT=1,  end 
else 

% - Direction  Update - 

gdnew=GRAD'*SD; 
if  OPTIONS/ 1)>0, 

num^spnntf/roS  Of  %  12.3g%12.3g  '.OPTIONS/ 1 0),f,OPTIONS(l  8)),... 
spnntfC%12.3g  ',gdnew)J; 
end 

if  /gdnew>0  &  P>FIRSTF)|~finite/f) 

%  Case  1 :  New  function  is  bigger  than  last  and  gradient  w.r.t.  SD  -ve 
%  ...interpolate. 
how='inter'; 

[stepsize  )=cubici  1  /f,FIRSTF,gdnew,GDOL  D.OPTJONS/ 1 8)); 
if  stepsize<0|isnan/stepsize),  stepsize=OPTIONS/18)/2;  how='Clf ';  end 
if  OPTIONS/  18)<0.l  &OPTIONS(6)==0 
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if  stepsize  *norm(  SD  )<eps 
rand('normal') 
stepsize=rand(  l ); 

how='RANDOM  STEPLENGTir. 
status=0, 

else 

stepsize=stepsize/2; 

end 

end 

OPTIONS(  1 8)=stepsize, 

XOU  T=OLDX; 
else  if  f<FlRSTF 

[newstep.fbest]  =cubici3(f,FIRSTF,gdnew,GDOLD,OPTIONS(  1 8)); 

sk=(XOUT-OLDX)'*(GRAD-OLDG); 

if  sk>le-20 

%  Case  2:  New  function  less  than  old  fun.  and  OK  for  updating  HESS 
%  ...  update  and  calculate  new  direction. 

how="; 

if  gdnew<0 

how-incstep'; 
if  newstep<OPTIONS(18) 

newstep=2*OPTIONS(  1 8)+ 1  e-5: 
how=[how,'  IF']; 
end 

OPTIONS!  1 8)=min([max({2>  1 . 5*OPTIONS(  1 8)  1),  1  +sk+abs(gdnew)+. . . 
max([0,OPTIONS(18)-l]),  (1.2+0  3*(~OPT!ONS(7)))*abs(newstep)]); 
else  %  gdnew>0 

if  OPTlONS(18)>0.9 
how='int_st'; 

OPTIONSf  1 8)=niin([  1  .abs(newstcp)  J); 

end 

end  %if  gdnew 

[HESS,SD]=updhess(XOUT,OLDX,GRAD.OLDG,HESS,OPTJONS); 

gdnew=GRAD'*SD; 

OLDX=XOUT; 

status=4; 

%  Save  Variables  for  next  update 
FIRSTF=f; 

OLDG=GRAD; 

GDOLD=gdnew; 

%  If  mixed  interpolation  set  PCNT 

if  OPTIONS(7)=0,  PCNT=1;  MATX=zeros(3,l);  MATL(  1  )=f;  end 
elseif  gdnew>0  %sk<=() 

%  Case  3:  No  good  for  updating  HESSIAN  ..  interpolate  or  halve  step  length. 
how=’inter_st'; 
ifOPTIONS(18)X).01 

OPTIONSf  1 8)=0.9*newstep; 

XOUT=OLDX; 

end 

if  OPTIONS(18)>l,  OPTIONS(18)=l  .  end 

else 

%  Increase  step,  replace  starting  point 

OPTIONS!  1 8)=max([nun([newstep-OPTIONS(  1 8),3]),0. 5*OPTIONS(  1 8)]); 
how=’incst2'; 

OLDX=XOUT; 

FIRSTF=f; 

OLDG=GRAD; 

GDOLD=GRAD'*SD; 

OLDX=XOUT; 
end  %  if  sk> 

%  Case  4:  New  function  bigger  than  old  but  gradient  in  on 
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%  reduce  step  length, 
else  %gdnew<0  &  F>FIRSTF 
if  gdnew<0&P»FIRSTF 
how-redstep'; 

if  norm(GRAD-OLDG)<le-10;  5IESS=eye(nvars);  end 
if  abs(OPTIONS(  1 8))<eps 
randCnormal') 

SD=norm(SD)*rand(SD) 

0PT10NS(  18)=abs(rand(l  ))*  le-6; 
how-RANDOM  SD'; 

else 

OPTIONS!  1 8)=-OPT10NS(  1 8)/2 ; 
end 

XOU  T=OLDX; 
end  %gdnew>0 

end  %  if  (gdnew>0  &  F>FIRSTF)|~finite(F) 

XOUT=XOUT+OPTIONS(  1 8)*SD, 
if  OPT10NS(1)>0 
%  disp([num,how)) 

end 

end  % - End  of  Direction  Update - 

%  Check  Termination 

if  max(abs(SD))<2*OPT10NS(2)  &  (GRAD'*(SD))  <  2*OPTIONS(3) 
ifOPTIONS(l)>0 
disp(");disp(");disp("); 
disp(");disp(");disp("); 

dispCOptimization  Terminated  Successfully'); 

%  dispf’Gradient  less  than  options!  2V). 

disp(['  NO  OF  ITERATIONS^,  num2str(OPTIONS(10))]), 

end 

status=l; 

el  seif  OPTIONSf  1 0)>OPTIONS(  1 4) 
if  OPTIONSf  1  )>=0 
dispO;disp(");disp("); 
disp(");disp(”);disp("); 

dispCWaming:  Maximum  number  of  iterations  has  been  exceeded'); 
dispC  -  increase  options(  14)  for  more  iterations. ') 
end 

status=l; 

else 

%  Line  search  using  mixed  polynomial  interpolation  and  extrapolation, 
if  PCNT~=0 

while  PCNT  >  0 

OPTIONSf  1 0)=OPTIONS(  1 0)+l ; 
disp(");disp(");disp("); 
dispCTermination  Check  in  Progress'); 
disp(['Iterations:  ',num2str(OPTIONS(  10))j); 
x(;)  =  XOUT; 

f  =  eval(evalsU); 

[PCNT,MATLMATX,steplenX  howJ=searchq(PCNT,f,OLDX,... 

MATL,MATX,SD,GDOLD,OPTIONS(18),  how); 
OPTIONSf  1 8)=steplen; 

XOUT=OLDX+steplen*SD; 
if  absfsleplen)  <  le-6,  PCNT=0;  status=l  ,  end 
end 


x(:)=XOUT; 

OPTIONS!  1 0)=OPTIONS(  1 0)+ 1 ; 
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dispC’);disp(");disp(“); 
dispCTermination  Check  in  Progress'); 
dispd'Iterations.  \num2str(  OPTIONS!  1 0))]); 
f  =  eval(evalstr); 
end 
end 
end 

x(:)=XOUT; 

dispO;disp(").disp("), 

disp(");disp("),disp("); 

disp(");dtsp("),disp("); 

disp('Final  Evaluation  in  Progress'); 

f  =  eval(evalstr); 

if  f  >  FIRSTF 

OPTIONS(8)  =  FIRSTF; 

x(:)=OLDX; 

else 

OPTIONS(8)  =  f; 
end 


F.  MainMin 

%  Filename  is  "MainMin. m“ 

%  This  is  the  routine  used  by  "MainOpt.m"  to  optimize  the  reference 
%  trajectory  polynomial  coefficients.  It  is  a  scaled  down  version 
%  of  the  dual  arm  spacecraft  program,  "Main2.m".  This  version  does 
%  not  integrate  the  state  variables  not  include  a  Lyapunov  controller. 

%  The  only  integration  that  does  take  place  is  the  optimization  cost 
%  function. 

function  [JOpt]  =  MainMin(lJpCoef,ConstMatJ-lags) 

%clg;clear, 

format  compact,format  short; 
k  =  length(UpCoef); 

AS43  =  inv(ConstMat(: Jk+1  :k+3))*([l ;  0;  0]  -  ConstMat(:,I:k)*UpCoef); 
Coef  =  [UpCoef ;  A543];  %  Reference  trajectory  polynomial  coefficients 

%  Reference  Maneuver  Start  and  Stop  Times 
TO  =  0; 

TfR  =  10; 

TfC  =  10; 

MetaFlag  =Flags(I); 

ContFlag  =  Flags(2); 

PertFlag  =Flags(3); 

AMatFlag  =  Flags{4); 

WheelFlag  =  Flags(5); 

EOMFlag  =Flags(6); 

PInvFlag  =Flags(7); 

KEFlag  -  Flags(8); 

OutFlag  =  Flags(9); 

Trace  =Flags(10); 

SymFlag  =  Flags(  1 1); 

ByPass  =  FIags(  1 2); 

TorqFlag  =Flags(13); 

Tol  =  le-6;  %  Integration  tolerance 
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R2D  =  180/pi;  %  Conversion  factor  from  radians  to  degrees 


%  Lengths  (m) 

L0  =  0.75;  %  Origin  to  left  shoulder 

L 1  =  0.5;  %  Left  upper  arm 

L2  =  0.5,  %  Left  forearm 

LP  =  0.5;  %  Payload 

R2  =  0.5;  %  Right  forearm 

R 1  =  0.5;  %  Right  upper  arm 

R0  =  sqrt(2*0.75A2);  %  Origin  to  right  shoulder 

Ls  =  [L0;  LI;  L2;  LP;  R2;  Rl;  R0], 

%  Member  masses  (kg) 

MO  =  5; 

ML1  =  1; 

ML2  =  1; 

MR1  =  1; 

MR2  =  1; 

MP  =  I; 

Ms  =  [MO;  ML  1;  ML2;  MR1;  MR2;  MP]; 

%  Center  of  mass  distances  (m) 

LcO  =0; 
lxLl  =0.25; 

LcL2  =  0.25; 

LcRl  =0.25; 

LcR2  =  0.25; 

LcP  =0.25; 

CMs  =  [LcO;  LcL  1 ;  LcL2;  LcRl;  LcR2;  LcP]; 

%  MOI  about  center  of  mass:  Ic  =  (1/1 2)*(mass)*(Iength)A2 
10  =  M0; 

%I0  =  0; 

IL1  =  (1/12)  *  ML1  *  LlA2; 

IL2  =  (1/12)  *  ML2  *  L2A2; 

IR1  =  (1/12)  *  MR1  *  R1A2; 

IR2  =  (1/12)  *  MR2  *  R2A2; 

IP  =  (1/12)  *  MP  *  LPA2; 

Is  =  [10;  IL1;  IL2;  IR1;  IR2;  IP]; 

%  Nominal  initial  and  desired  final  locations  of  payload 
%  Point  Q  is  at  wrist  of  left  arm 
%  Point  P  is  at  wrist  of  right  arm 
QxOn  =  0.125;  Qy0n=t.5; 

Px0n  =  0.625;  Py0n=1.5; 

Qxf  =0.125;  Qyf  =  1.0; 

Pxf  =0.125;  Pyf  =  1.5; 

%  Nominal  initial  and  desired  final  locations  of  centerbody 
ThOO  =  0; 

ThOf  =  0/R2D; 

%  Arms  mount  locations  wrt  centerbody  coordinate  frame  (rad) 
ThLO  =  pi/2; 

ThRO  =  pi/4; 

AngConst(  1 )  =  ThLO; 

AngConst(2)  =  ThRO; 

%  Symmetric  geometry  to  center  arms  and  test  kinetic  energy 
if  SymFlag 
ThLO  =  3*pi/4; 


127 


AngConst(l)  =  ThLO; 

RO  =  LO;  %  Ongin  to  nght  shoulder 
l.s(7,l)  =  RO; 

QxOn  =  -0  25.  QvOn  =  12: 

PxOn  =  0.25.  PyOn  =  1 .2; 
end 

BoundC(l.l)  =  QxOn;  BoundC(l,2)  =  QyOn; 

BoundC(2, 1 )  =  PxOn,  BoundC(2,2)  =  PyOn. 

BoundC(3,I)  =  Qxf;  BoundC(3,2)  =  Qyf; 

I  toundC(4, 1 )  =  Pxf.  BoundC(4,2)  =  Pv  I, 

BoundC(5, 1 )  =  ThLO;  BoundC(5,2)  =  IhRO; 

I  loundC(6, 1 )  =  TO;  BoundC(6,2)  =  1TR; 

BoundC(7, 1 )  =  TfC;  BoundC(7,2)  =  ContFlag; 

BoundC(8,l)  =  AMatFlag;  BoundC(8,2)  =  WheelFlag; 

1  )oundC(9, 1 )  =  ThOO;  BoundC(9,2)  =  IhOf; 

BoundC(10.1)=  l-OMFlag;  BoundC(10,2)=  PlnvFiag; 

BoundC(l  1,1)=  KEFlag;  BoundC(l  1,2)=  ByPass; 

BoundC(  1 2, 1  )=  TorqFlag; 

%  Weighting  Matrices 

%  Control  torques  are  calculated  to  minimize  the  following  cost  function; 

%  J  =  0.5*(u'*Wu*u  +  (A'*Lam),*Wc*(A'*Lam)) 
if  WheelFlag 

Wu  =  eye(7);  %  Control  Torque  Weighting 

else 

Wu  =  eye(6); 
end 

%if  WheelFlag 
%  Wu  =  zeros(7,7); 

%else 

%  Wu  =  zeros(6,6); 

%end 

%Wu(4,4)=le5; 

%Wu(7,7)=le5; 

%Wu(2,2)=lel0; 

%Wu(5,5)=lelO; 

Wc  =  zeros(8,8);  %  Constraint  Force  Weighting 
%Wc  =  eye(8); 

%%%%%%%%%%%%%%%%% 

%%  INITIAL  CONDITIONS  %% 

%%%%%%%%%%%%%%%%% 

%  Desired  Initial  Payload  Parameters 
ThPO  =  atan2(Py0n-Qy0n,Px0n-Qx0n); 

XcO  =  0.5  *  (PxOn  +  QxOn); 

YcO  =  0.5  *  (PyOn  +  QyOn); 

QxO  =  QxOn; 

QyO  =  QyOn; 

PxO  =  PxOn; 

PyO  =  PyOn; 

%  Initial  State 
XO  =  0; 

%%%%%%%%%%%%% 

%%  INTEGRATION  %% 

%%%%%%%%%%%%% 

%  RefMin2  uses  change  in  angular  momentum  to  find  wheel  command  torque 
fT,JInt,J]  =  odemin(,Re{Min2',T0,TfR,X0,Tol,Trace.Ls,Ms,CMs,Is,BoundC,... 
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Wu.Wc.Coef.ConslMat). 

%  Optimization  cost  function  is  integral  of  J 
k  =  lcngth(T), 

JOpt  =  Jlnt(k); 

%JOpt  =  max(abs(J)), 


G.  MainOpt 

%  Filename  is  "  MainOpt. m'1 

%  This  routine  optimizes  the  dual  arm  spacecraft  cost  function 
%  hv  changing  the  polynomial  coefficients  which  describe  the 
%  reference  trajectory.  It  calls  "Main2.m“ 

%clcar 

clc 

home 

format  compact 
format  short 

1  IpCoelX)  =  (0);  %  Starting  Guess 

%l  IpCoefO  =  UpCoef:  %  Use  last  values  for  starting  guess 

k  =  iength(UpCoelD); 

options  =  [J;  %  Default  values 

options(l)  =0;  %  Display  during  optimization  cycle:  l=On,  0=OIT 

options(  1 4)  =  1 00*k;  %  Maximum  number  of  iterations 

%%%%%%%%%%%%%%%%%%%%%“/»%%%%%%%%%%% 

%%  Flags  during  optimization  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
MetaFlag  =  0;  %  Creates  metafile  named  "main.met" 

ContFlag  =  0;  %  Controller  Status  Flag:  l=On;  0=0 (T 

PertFlag  =  0;  %  Perturbation  Flag  (0=no  perturbation.  1  =perturbation) 

%  The  perturbation  is  to  check  the  controller  by 
%  disturbing  the  actual  initial  state  away  from  nominal. 

%  The  reference  torques  are  based  on  nominal. 

AMatFlag  =  0;  %  Size  of  A  matrix:  0  =  4x8  (Free  Centerbodv) 

%  1  =  5x8  (Fixed  Centerbodv) 

WheelFlag  =  1 .  %  Centerbodv  Reaction  Wheel  ( 1  =On,  0=Off) 

EOMFlag  =  8;  %  Specifies  number  of  cost  function  constraint  equations 
%  3  =  only  payload  equations 

%  5  =  only  spacecraft  equations 

%  any  other  value  =  all  8  equations 

PInvFlag  =  1 ;  %  Psuedo-inverse  Flag  (for  use  in  finding  reference  torques) 
%  1  =  Use  psuedo-inverse 

%  0  =  Use  inverse 

KEFIag  =  0;  %  KE  Test  Flag 

%  1  =  Nonzero  velocity  initial  conditions 

%  0  =  Zero  velocity  initial  conditions 

OutFlag  =  0;  %  Output  Flag 

%  1  =  Display  output 

%  0  =  Don't  display  output 

Trace  =1;  %  Observe  integration 

%  1  =  Observe 

%  0  =  Don't  observe 

OptFlag  =  0;  %  Optimization  Flag 

%  1  =  Perform  optimization 

%  0  =  Don't  perform  optimization 

SymFlag  =  0;  %  Symmetric  Geometry  Flag 
%  1  =  Symmetric  geometry 
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%  0  =  Nonsvmmetne  geometry 

I  iv  Pass  =0,  %  Torque  calculation  bypass  (lag 

%  I  =  Bypass 
%  0  =  Use  inverse  kinematics 

TorqlTag=  0.  %  Torques  to  use  if  bypass  enabled 
%  0  =  No  Torques  (Drift) 

%  1  =  One  Shoulder  Torque 

%  2  =  Symmetric  Shoulder  Torques 

TorqCap  =  0.  %  Maximum  limit  on  wheel  torque 
%  I  =  Enabled 
%  0  =  Disabled 

TorqMax  =  0075;%  Limit  on  wheel  torque  if  TorqCap  enabled 
llagsl(l)  =  Metal-Tag; 

I- lags  1(2)  =  ContFlag; 
f  lags  I  (3)  =  PertFlag; 

Flags! (4)  =  AMatFlag; 

Flags  1(5)  =  WheelFlag; 

Flags  1(6)  =  EOMFlag; 

Flags  I  (7)  =  ITnvFlag; 

Flags  1(8)  =  KEFlag; 

ITagsl(9)  =  OutFlag; 

Flags  1(10)=  Trace. 

Flags  I  ( 1 1  )=  SymFlag; 
lTagsl(12)=  ByPass. 

Flagsl(13)=  TorqFlag; 

ITagsl(14)=  TorqCap; 

Flagsl(15)=  TorqMax, 

%%%%%%%%%%%%%%%% 

%%  Flags  after  optimization  %% 

%%%%%%%%%%%%%%%% 

Flags2  =  Flags  1 ; 

Flags2(l)  =  0;  %MetaFlag:  l=On,  0=OIT  (File  is  "main. met") 

Flags2(2)  =  1 ;  %  Controller  Flag:  l=On,  0=01T 

Flags2(3)  =  0;  %  Perturbation  Flag:  1  =On,  0=O(T 

Flags2(5)=  1;  %  Wheel  Flag:  l=On,0=OIT 

Flags2(8)  =  0;  %  Kinetic  Energy  Flag:  l=On.  0=()(T 

Flags2(9)  =  1 ;  %  Output  Flag:  l=On.  0=Off 

Flags2(  1 0)=  1 ;  %  Trace  Flag:  1  =On,  0=OIT 

Flags2(  1 1  )=  0.  %  Symmetric  Geometry  Flag:  l  =Sym,  0=NonSv m 

Flags2(  1 2)=  0;  %  Inverse  Kinematics  Bypass:  1  =Bvpass,  ()=Inverse  Kinematics 

Flags2(  1 3 )=  0;  %  Torq  Flag:  0=No  Torq,  I  =One  Torq,  2= Two  Symmetric  Torqs 

%  Torq  Flag  is  for  when  the  bvpass  is  enabled 
Flags2(  1 4)=  0;  %  TorqCap:  1  =On.  0=OlT 

ITags2(15)=  0.075',%  Limit  on  maximum  wheel  torque 
Flags2(  16)=  0;  %  DocFlag:  l=On,  0=Off 

%  separate  meta  files  for  each  page  ("doc#.met") 

Dial-Tag  =  0;  %  Diary  Flag 

%  I  =  Create  diary  file  "mnin.dia" 

%  0  =  No  diary  file 

ConstMat  =  ones(3,k+3); 
forn=l:k+3 
ConstMat(2,n)  =  k+6-n; 

ConstMat(3,n)  =  ConstMat(2,n)*(ConstMat(2,n)-l); 
end 

if  OptFlag 

[  UpCoef .options]  =  fminu2('MainMin',UpCoel0,options,|  ],ConstMat.Flagsl); 
end 

if  DiaFlag 
diary  main.dia 
end 
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if -OptFlag 
UpCoef®  UpCoelt); 
end 

[  JInt)  =  Main2(UpCoef,ConslMat,Flags2); 

%  Plot  position,  velocity,  &  acceleration  reference  traieciones 
k  =  lcngth(UpCoef). 

A543  =  inv(ConstMat(:,k+l  k+3))*([l;  0;  0|  -  ConsiMat(:,!:k)*UpCoef ). 
Coef  =  [UpCoef;  A543  j;  %  Reference  trajectory  polynomial  coetficients 
k  =  length(Coef); 

Steps  =  21; 
lorm=  l  Steps 
Tau  =  (m- 1  )/(Steps- 1 ); 
forn=l:k 

CTau(k+l-n)  =  Coef(k+ 1  -n)*TauA(n+2), 

CTaud(k+l-n)  =  Cocf(k+l-n)*TauA(n+l); 

CTaudd(k+l-n)  =  Coef(k+l-n)*TauA(n), 
end 

W(m)  =  ConstMat(l,.)*CTau'; 

Wd(m)  =  ConstMat(2,;)*CTaud’, 

Wdd(m)  =  ConstMat(3,:)*CTaudd'; 
end 
elg 

T=():l  /(Steps- 1 ):  1 ; 
suhplot(22 1 ) 

plot(T,W),title('Position  vs  Normalized  Time  ); 
xlabel('Tau  (sec)');y  label('Position'); 
subplot(222) 

title('Reference  Trajectories') 
subplot(223) 

plot(T,Wd);title(’Velocity  vs  Normalized  Time'). 
xlabelf'Tau  (sec)'),ylabei('Vel,  ;ity'); 
subplot(224) 

plot(T,Wdd);title(' Acceleration  vs  Normalized  Time'); 
xlabel(Tau  (sec)');ylabel('Acceleration'); 
if  Flags2(  1 ) 
meta  main 
end 

if  Flags2(16) 
metadoc6 
end 
pause 

dispf 'Initial  guess  for  highest  order  coefficients');disp(UpCoeR)'); 
disp('Coefiicients  in  descending  order');disp(Coef). 
disp(Tntegral  of  Cost  Function,(JIntAbs  &  JInlSqr)’);disp(JInt); 
if  OptFlag 

disp(Tterations’);disp(options(10)); 

end 

diary  off 


H.  Main2 

%  Filename  is  "Main2.m" 

%  This  routine  is  the  driver  for  the  dual  arm  spacecraft  problem 
%  but  is  called  by  "MainOpt.m"  after  the  polynomial  reference  trajectory 
%  coefficients  have  been  optimized. 
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function  [JIntTotal|  =  Main2(UpCoef,ConstMat.Flags) 

%  Calculate  the  coefficients  for  orders  five.  four,  and  three 
%  Include  these  with  the  higher  order  coefficients  in  a  vector 
k  =  length(UpCoef); 

A54.1  =  inv(ConstMat(:,k+l :k+3))*([l;  0;  0|  -  C'onslMat(:,l:k)*UpC'oef); 
C'oef  =  (UpCoef ;  A543],  %  Reference  trajectory  polynomial  coelficients 


%%%%%%%% 

%%  Times  %% 

%%%%%%%% 

%  Reference  Maneuver  Start  and  Stop  Times  and  Controller  Stop  l  imes 
%  Setting  the  controller  time  longer  than  the  reference  maneuver  time 
%  ensures  that  the  controller  eliminates  any  errors  remaining  after  the 
%  reference  trajectory  should  be  complete.  To  exercise  the  controller 
%  only  with  no  reference  trajectory,  set  the  reference  maneuver  stop 
%  time  to  a  negative  value. 

TO  =  0; 

TfR  =  10. 

TfC  =  10, 

MetaFlag  =Flags(l); 

ContFlag  =  Flags(2); 

PertFlag  =  Flags(3); 

AMatFlag  =  Flags(4); 

WheelFlag  =  Flags(5); 

FOMFlag  =  Flags(6); 

PInvFlag  =  Flags(7); 

KEFlag  =  Flags(8); 

OutFlag  =  Flags(9); 

Trace  =Flags(10); 

SvmFlag  =  Flags(  1 1 ); 

ByPass  =Flags(12); 

TorqFlag  =Flags(13); 

TorqCap  =Flags(14); 

TorqMax  =Flags(15); 

DocFlag  =Flags{16); 


Pert  =  -10;  %  Perturbation  of  initial  payload  angle,  ThetaP  (deg) 
Tol  =  le-6;  %  Integration  tolerance 

Interval  =  3,  %  Stick  figure  drawing  includes  every  Interval'th  time 
R2D  =  180/pi;  %  Conversion  factor  from  radians  to  degrees 


%%%%%%%%%%%%%% 


%%  System  Parameters  %% 
%%%%%%%%%%%%%% 


%  Lengths  (m) 

L0  =  0.75;  %  Origin  to  left  shoulder 

L 1  =0.5;  %  Left  upper  arm 

L2  =  0.5;  %  Left  forearm 

LP  =  0.5;  %  Payload 

R2  =  0.5;  %  Right  forearm 

R 1  =  0.5;  %  Right  upper  arm 

R0  =  sqrt(2*0.75A2);  %  Origin  to  right  shoulder 

Ls  =  [L0;  LI;  L2;  LP;  R2;  R 1 ;  R0]; 


%  Member  masses  (kg) 
M0  =5; 

ML1  =  1; 

ML2  =  1; 

MR1  =  1; 
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MR2  =  1; 

MP  =  I; 

Ms  =  (MO;  MU  ,  ML2;  MR1;  MR2;  MP(; 

%  Center  of  mass  distances  (m) 
l.cO  =0; 
l.cl.l  =0.25. 

I  el, 2  =0.25; 

I.cRI  =0.25; 
l.cR2  =  0  25; 

I. eP  =0.25; 

CMs=  (LcO;  LcL  1 ;  LcL2;  LcRl;  LcR2;  LeP|; 

%  MOI  about  individual  centers  of  mass 

%  Arms  are  modelled  as  slender  rods:  Ic  =  ( l/12)*(mass)*(length)A2 
10  =  MO; 

II. 1  =  (1/12)  *  MLl  *  L1A2; 

11.2  =  (1/12)  *ML2*L2A2; 

1RI  =  (1/12)  *  MRl  *  R1A2; 

1R2  =  (1/12)  *  MR2  *  R2A2; 

IP  =  ( I/I 2)  *  MP  *  LPA2; 

Is=  (10;  1L 1 ;  IL2;  IR1;  IR2;  IP], 

%  Nominal  initial  and  desired  final  locations  of  payload 
%  Point  Q  is  at  wrist  of  left  arm 
%  Point  P  is  at  wrist  of  right  arm 
QxOn  =  0. 1 25;  Qy0n=15; 

PxOn  =0.625;  Py0n=1.5; 

Qxf  =0.125;  Qyf  =  1.0; 

Pxf  =0.125;  Pyf  =  1.5; 

%  Nominal  initial  and  desired  final  locations  of  centerbodv 
ThOO  =  0/R2D; 

ThOf  =  0/R2D; 

%  Arms  mount  locations  wrt  centerbody  coordinate  frame  (rad) 

Thl.O  =  pi/2; 

ThRO  =  pi/4; 

AngConst(  1 )  =  ThLO; 

AngConst(2)  =  ThRO; 

%  Symmetric  geometry  to  center  arms  and  test  kinetic  energy 
if  SvmFlag 
ThLO  =  3*pi/4; 

AngConst(  1 )  =  ThLO; 

R0  =  L0;  %  Origin  to  right  shoulder 
Ls(7,l)  =  R0; 

QxOn  =  -0.25;  Qy0n=l.2; 

PxOn  =  0.25;  PyOn=  1.2; 
end 


%  Assemble  information  required  in  other  subroutines  into  a  matrix 


BoundC(  1,1)  =  QxOn; 
BoundC(2,l)  =  PxOn; 
BoundC(3,l)  =  Qxf; 
BoundC(4,l)  =  Pxf; 
BoundC(5,l)  =  ThLO; 
BoundC(6, 1 )  =  TO; 
BoundC(7,  l )  =  TfC; 
BoundC(8,l)  =  AMatFlag; 
BoundC(9,l )  =  ThOO; 


BoundC(  1 ,2)  =  QyOn; 
BoundC(2,2)  =  PyOn; 
BoundC(3,2)  =  Qyf; 
BoundC(4,2)  =  Pvf; 
BoundC(5,2)  =  ThRO; 
BoundC(6,2)  =  TfR; 
BoundC(7,2)  =  ContFlag; 
BoundC(8,2)  =  WheelFlag; 
BoundC(9,2)  =  ThOf; 
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BoundC(H),  1)=  LOMFlag.  BoundC(  10,2)=  Plnvllag. 

BoundC'i  1 1 , 1  )=  KEFlag,  BoundCf  1 1,2)=  Bvl’ass. 

BoundC(  12, 1  }=  TorqFlag; 

BoundC(  13,!)=  TorqCap,  BoundC(  1 3,2)=  TorqMax. 

%  (lip  are  gains  for  angle  i  position  error 
%  Giv  are  gains  lor  angle  i  velocity  error 
( ipos  =  0  5;  %  Position  error  gain 

( ivel  =  0  2.  %  Velocity  error  gain 

(iains  =  |Gpos;  Gvel); 

%  Weighting  Matrices 

%  Control  lorques  are  calculated  to  minimize  the  following  cost  function: 
%  J  =  0  5*(u'*Wu*u  +  (A'*Lam)'*Wc*(A'*l.ami) 

%  Wu  is  the  control  torque  weighting  matrix 
%  Wc  is  the  constraint  force  weighting  matrix 
if  WheelFlag 

Wu  =  eye(7),  %  Control  Torque  Weighting 

else 

Wu  =  eve(6), 
end 

%if  WheelFlag 
%  Wu  =zeros(7,7); 

%else 

%  Wu  =  zeros(6,6); 

%end 

%Wu(4,4)=le5.  %  Penalty  on  wrist  motors  for  free  ccntcrhodv  case 

%Wu(7,7)=le5; 

%Wu(2,2)=le  10;  %  Penalty  on  wrist  motors  for  fixed  centerbody  case 

%Wu(5,5)=lelO; 

Wc  =  zeros(8,8),  %  Constraint  Force  Weighting 
%Wc  =  eye(8); 


%%%%%%%%%%%%%%%%% 

%%  INITIAL  CONDITIONS  %% 

%%%%%%%%%%%%%%%%% 

%  Desired  Initial  Payload  Parameters 
ThPO  =  atan2(Py0n-Qy0n,Px0n-Qx0n); 

XcO  =  0.5  *  (PxOn  +  QxOn), 

YcO  =  0.5  *  (PyOn  +  QyOn); 

if  PertFlag  %  Perturbation  enabled 

ThPO  =  ThPO  +  Pert/R2D;  %  Perturb  payload  angle 
QxO  =  XcO  -  LcP*cos(ThP0);  %  Perturb  arm  end  points 
QyO  =  YcO  -  LcP*sin(ThPO); 

PxO  =  XcO  +  (LP-LcP)*cos(ThPO); 

PvO  =  YcO  +  (LP-LcP)*sin(ThPO); 
else  %  No  Perturbation 

QxO  =  QxOn; 

QyO  =  QyOn; 

PxO  =  PxOn; 

PyO  =  PyOn; 
end 

PertCrd  =  [QxO  QyO  PxO  PyO]; 

%  Left  Arm 

%  Elbow  is  left  of  line  from  arm  base  to  Q  (RQ) 

LSx  =  LO  *  cos(ThOO  +  ThLO); 

LSv  =  LO  *  sinfThOO  +  ThLO); 

RQ  =  sqrt((QxO-LSx)A2-KQyO-LSy)A2);  %  Length  from  arm  base  to  Q 
Betal  =  atan2(QyO-LSy,QxO-LSx);  %  Angie  from  aim  base  to  RQ 
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%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  a A2  V{  2l>c ) 

%  Apply  to  find  angle  between  RQ  and  Link  L 1 
Num  =  LlA2  +  RQA2-L2A2; 

Den  =  2  *  L 1  *  RQ. 

Beta2  =  acos( Num/Den );  %  Angle  from  RQ  lo  Link  1 

ThL  10  =  (Betal  +  Beta2)  -  (ThOO  +  ThLO);  %  Theta  L 1 
%  l  Ise  law  of  cosines  to  find  the  interior  angle  at  the  elbow 
Num  =  L 1 A2  +L2A2  -  RQA2 
Den  =  2  *  LI  *  L2; 

Beta3  =  acos(  Num/Den): 

ThL20  =  -(pt-Beta3), 


%  Right  Arm 

%  Elbow  is  right  of  line  from  arm  base  (shoulder!  to  I’  (wrist )  (RP) 

RSx  =  RO  *  cos(Th00  +  ThRO), 

RSv  =  RO  *  sind'hOO  +  ThRO): 

RP  =  sqrt((PxO-RSx)A2+(PyO-RSy)A2):  %  Length  from  arm  base  to  P 
Betal  »  atan2(Py0-RSy.Px0-RSx);  %  Angle  from  arm  base  to  RP 
%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RP  and  Link  R1 
Num  =  R1A2  +  RPA2  -  R2A2; 

Den  =  2  *  R 1  *  RP. 

13eta2  =  acos(Num/Den);  %  Angle  from  Link  R 1  to  RP 

Beta4  =  Beta  1  -  (ThOO  +  ThRO); 

ThR  10  =  -(Beta2  -  Beta4), 

Num  =  R1A2  +  R2A2  -  RPA2; 

Den  =  2  *  R I  *  R2. 

Beta3  =  acos(  Num/Den); 

ThR20  =  pi  -  Beta3; 

%  Desired  Initial  State 

X0  =  [ThOO;  ThL  10,  ThL20;  ThRlO;  ThR20;  ThPO;  XcO;  YcO; ... 

0;  0;  0;  0;  0;  0;  0;  0J; 

%%%%%%%%%%%%%%%%%%%% 

%%  Kinetic  Energy  Test  Conditions  %% 
%%%%%%%%%%%%%%%%%%%% 

%  Specify  Payload  and  Centerbody  Initial  Rates 
%  Compatible  Rates  for  the  Redundant  Coordinates  are  calculated 
if  KEFlag 

ThPdO  =  0/R2D;  %  Rates  to  specify 
XcdO  =  -0.03; 

YcdO  =  -0.°3; 

ThOdO  =  0/R2D; 

%%%%%%%%%%% 

%%  LEFT  ARM  %% 

%%%%%%%%%%% 

%  [Qxd:  Qyd|  =  [Hl]*Th0d  +  [H2]*Thd 
%  Qxd  &  Qyd  are  x  &  y  components  of  point  Q  inertial  velocity. 

%  Thd  =  [ThL  1  dot;  ThL2dot] 

%  H  matrices  are  made  from  expressing  the  x  &  y  components  of  Q  in 
%  terms  of  L0,  ThO,  ThLO,  L 1 ,  ThL  1 ,  L2,  and  ThL 2. 

%  Qx=L0*cos(Th0+ThL0)+L  1  *cos(Th0+ThL0+TL  1  )+L  2*cos(Th0+. 

%  ThLO+ThL 1 +ThL2) 

%  Qy=L0*sin(Th0+ThL0)+L  1  *sin(ThO+ThLO+lTiL  1  )+L2*sin(Th0+. 

%  ThLO+ThL l+ThL2) 

%  The  differentiation  of  these  equations  lead  to 
%  [Qxd;  Qyd]  =  [Hl]*Th0d  +  [H2]*Thd  which  can  be  solved  for  Thd 
QxdO  =  XcdO  +  LcP  *  ThPdO  *  sin(ThPO); 

QydO  =  YcdO  -  LcP  *  ThPdO  *  cos(ThP0); 

112(1,2)  =  -L2*sin(Th00+ThL0+ThL  1 0+ThL20); 
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1 12(  1, 1 )  =  H2(  1,2)  -  L  i  *stn( ThOO+ThLO+ThI .10). 

1 12(2.2)  =  l.2*cos(Th(X)+ThLO+ThL  lO+Thl.20). 

1 12(2, 1 )  =  1 12(2,2)  +  L  l*cos(ThOO+ThLO+Thl  10). 

111(1,1)=  112(1,1)-  I.O*sin(ThOO+ThU». 

111(2,1)  =  112(2,1)  +  1 ,0*cos(  ThOO+ThL  0 ); 

TIulO  =  inv(H2)  *  ([QxdO.  QydO|  -  Ill*ThOdO); 

%  Angle  rates 
Thl.ldO  =  rhdO(l), 
lhl.2dO  =  ThdO(2), 

%%%%%%%%%%%% 

%%  RIGHT  ARM  %% 

%%%%%%%%%%%% 

%  The  development  is  similar  to  the  left  arm 
%  Px=RO*cos(ThO+ThRO)+R  1  *cos(Th<>+ThRO+ThR  1  )+R2*cos(  Th()+ 

%  ThR0+ThRl+ThR2) 

%  Py=RO*sin(ThO+ThKO)+R  1  *sm(ThO+ThRO+ThR  1  )+R2*sin(  ThO+ 

%  ThRO+ThP  l+ThR2) 

%  [Pxd;  Pyd]  =  [Hl]*ThOd  +  [H2]*Thd 
PxdO  =  XcdO  -  (LP  -  LcP)  *  ThPdO  *  stn(ThPO); 

PvdO  =  YcdO  +  (LP  -  LcP)  *  ThPdO  *  cos(ThPO); 

I  i2(  1,2)  =  -R2*sin(ThOO+ThRO+ThR10+ThR2<)). 

112(1,1)  =  H2(  1 ,2)  -  R 1  *sin( ThOO+T hRO+Th R 1  <»: 

1 12(2,2)  =  R2*cos(Th(X)+ThR()+ThR10+ThR20). 

1 12(2, 1 )  =  1 12(2,2)  +  R  |  *cos(Th(X)+ThRO+ThR  10). 

111(1,1)=  H2(  1,D-  RO*stn(ThOO+ThRO); 

111(2,1)=  H2(2, 1 )  +  RO*cos(ThOO+ThRO); 

ThdO  =  inv(H2)  *  ([PxdO;  PydO]  -  Hl*ThOdO). 

%  Angle  rates 
ThRldO  =  ThdO(l); 

I  hR2dO  =  ThdO(2); 

X()  =  (ThOO,  ThLIO,  ThL20,  ThRlO,  ThR20;  ThPO;  XcO:  YcO:... 
ThOdO.  ThLldO,  ThL2dO;  ThRldO,  ThR2dO.  TltPdO.  XcdO;  Ycd()|; 
end 


%%%%%%%%%%%%%%%% 

%%  FINAL  CONDITIONS  %% 

%%%%%%%%%%%%%%%% 

%  Desired  Final  Payload  Angle 
ThPf  =  atan2(Pyf-Qyf,Pxf-Qxf), 

%  Left  Arm 

%  Elbow  is  left  of  line  from  arm  base  to  Q  (RQ) 

I.Sx  =  1.0  *  cos(ThOf  +  ThLO), 

I.Sv  =  LO  *  sin(ThOf  +  ThLO); 

RQ  =  sqrt((Qxf-LSx)A2+(Qyf-LSv)A2);  %  Length  from  arm  base  to  Q 
(Beta  1  =  atan2(Qyf-LSv,Qxf-LSx);  %  Angle  from  arm  base  to  RQ 

%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RQ  and  Link  L 1 
Num  =  LlA2  +  RQA2-L2A2; 

Den  =  2  *  L 1  *  RQ; 

Reta2  =  acos(Num/Den);  %  Angle  from  RQ  to  Link  1 

ThLlf  =  (Beta  1  +  Beta2)  -  (ThOf  +  ThLO);  %  Theta  L 1 
%  Use  law  of  cosines  to  find  the  interior  angle  at  the  elbow 
Num  =  L 1 A2  +  L2A2  -  RQA2; 

Den  =  2  *  L 1  *  L2; 

Beta3  =  acos(Num/Den); 

ThL2f=-(pi-Beta3); 

%  Right  Arm 

%  Elbow  is  right  of  line  from  arm  base  (shoulder)  to  P  (wrist)  (RP) 

RSx  =  RO  *  cos(ThOf  +  ThRO); 
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RSv  =  RO  *  sin(ThOf  +  ThRO); 

RP  -  sqrt((Pxf-RSx)A2+(Pyf-RSy)A2),  %  Length  from  arm  base  to  P 
Betal  =  atan2(Pyf-RSy,Pxf-RSx);  %  Angle  from  arm  base  to  RP 
%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RP  and  Link  R1 
Num  =  R 1 A2  +  RPA2  -  R2A2; 

Den  =  2  *  R1  *  RP, 

Beta2  =  acos(Num/Den);  %  Angle  from  Link  R1  to  RP 

Beta4  =  Beta  1  -  (ThOf  +  ThRO), 

ThRlf  =  -(Beta2  -  Beta4); 

Num  =  R 1 A2  R2A2  -  RPA2; 

Den  =  2  *  RI  *  R2: 

Beta.!  =  acos(Num/Den): 

ThR2f  =  pi  -  Beta3; 

%  Desired  Final  State 
Xcf  =  0.5  *  (Pxf  +  Qxf); 

Ycf  =  0.5  *  (Pvf  +  Qyf); 

QfDes  =  [ThOf,  ThLlf;  ThL2f,  ThRlf;  ThR2f,  ThPf:  Xcf.  Ycf; ... 

0;  0;  0;  0;  0;  0;  0;  0|; 

if  OutFlag 

%%%%%%%%%%%%%%%%% 

%%  PROBLEM  SUMMARY  %% 

%%%%%%%%%%%%%%%%% 
dispCPROBLEM  SUMMARY') 
dtsp(") 

disp('lnitial  Angles  (deg)1) 
dispCImtial  Angular  Rates  (deg/sec)') 
disp('Desired  Final  Angles  (deg)') 

disp('  ThetaO  ThetaLl  ThetaL2  ThetaRl  ThclaR2  ThetaP') 

disp(X0(  1 :6)'*R2D) 

disp(X0(9: 1 4)'*R2D) 

disp(QfDes(  1 :6)'*R2D) 

disp(") 

dispf'Payload  Coordinates  (m)') 

disp('  Nominal  Initial,  Perturbed  Initial,  and  Final') 

disp('  Qx  Qy  Px  Py') 

TableCrd  =  |Q.\0n  QyOn  PxOn  PyOn;  PertCrd;  Qxf  Qyf  Pxf  Py fj; 

disp(TableCrd) 

disp(") 

disp('Arm  Mounting  Locations  wrt  Centerbodv  Coordinate  Frame  (deg)') 

disp(ThL0*R2D);disp(ThR0*R2D) 

disjX") 

disp('Start,  Reference  Manuever  Stop,  &  Simulation  Stop  Times  (sec)') 

disp(T0);disp(TfR);disp(TfC) 

disp(") 

disp('Controller  Status  (1  =  On;  0  =  Of!)') 

disp(ContFlag) 

disp('') 

disp('Perturbation  Status  (1  =  On,  0  =  Off)') 

disp(PertFlag) 

disp(”) 

disp('Centerbody  Status  in  Forward  EOM  (1  =  Fixed;  0  =  Free)') 

disp(AMatFlag) 

disp(") 

disp('Reaction  Wheel  Status  (1  =  On,  0  =  Off)') 

disp(WheelFlag) 

disp(") 

disp('Number  of  Equations  in  Cost  Function  Constraint  (3,  5  or  8)') 
disp(EOMFIag) 
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dispO 

disp('Psuedo-Inverse  Status  ( 1  =  On;  0  =  Off)') 

disp(PlnvFlag) 

disp(") 

dispt'Nonzcro  Initial  Velocity  Status  (I  =  On;  0  =  <  >10') 

disp(KEFlag) 

disp(") 

dispCGeomeirv  Status  ( 1  =  Symmetric;  0  =  Nonsvmmetnc)') 

disp(SymFlag) 

disp(") 

disp(‘lnverse  Kinematics  Bypass  Status  ( 1  =  Bypass.  0  =  Use  inv.  kinematics)') 

disp(BvPass) 

dispt") 

disp( 'Torques  if  Bypass  Enabled  (0=None,  l=One,  2=  Two  Symmetric )') 

disp(TorqFlag) 

disp{") 

disp{'Reaction  Wheel  Torque  Cap  Status  ( I  =Enabled.  ()=Oisabled)') 

disp(TorqCap) 

if  TorqCap 

disp('Limit  on  Wheel  Torque’); 
disp(TorqMax); 
end 
dispi") 

dispCController  Gains  (position  and  velocity  )') 

disp('  Gpos  Gvel') 

disp(Gains') 

disp(") 

disp('Cost  Function:  J  =  ().5*(uT*Wu*u  +  (AT*Lam)T*Wc*(AT*Lam))') 

disp('  where T  signifies  transpose’) 

dispCControl  Torques  Weighting  Matrix,  Wu’) 
disp(Wu) 

%disp('Constraint  Forces  Weighting  Matrix,  We’) 

%disp(Wc) 

end  %  End  of  OutFlag  branch 

%%%%%%%%%%%%% 

%%  INTEGRATION  %% 

%%%%%%%%%%%%% 

%  "ode"  is  a  variable  step  size  Runge-Kutta  integrator  function 
%  supplied  with  MATLAB.  "ode2"  is  the  same  as  "ode"  in  its  function 
%  but  permits  the  passing  of  more  variables  into  and  out  of  the  function. 
[T,X,Torq,TorqRef,Aqdot,J,Res,LHS,RHS,Delql  =  ... 

ode2('Eqn2’,T0,TfC,X0,Tol,Trace,Ls,Ms,CMs.is.BoundC.... 
Gains,QlDes,Wu,Wc,Coef,ConstMat); 
k  =  length(T); 

.Hnt  =  X(:,17: 18); 

JIntTotal  =  X(k,  1 7: 1 8); 

if  OutFlag 

%%%%%%%%%% 

%%  OUTPUT  %% 

%%%%%%%%%% 

clg; 

%  Angle  Histories 
n  =  length(T); 

Q  =  X(:,1:6); 
subplot(221) 

plot(T,Q(:,  1  )*R2DJ,Q(:,2)*R2D,T,Q(:,3)*R2D.  . 
T,Q(:,4)*R2D,T,Q(:,5)*R2D,T,Q(:,6)*R2D); 
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hold  on 

plot(T(n),QfDes(l)*R2D,'*',T(n),QIDes(2)*R2D.’*'.T(n),QfDes(3)*R2D.'*’, .. 

^(n),QfDes(4)*R2D,,*•.T{n),QfDes(5)*R2D,,*M(n).QlDes(6)*R2D.,*'). 
tillc( ‘Thetas  vs  Time'), 

\label('Time  (sec)');ylabel(' Angles  (deg)'), 
hold  off 

%  Angle  Rate  Histones 
Qdot  =  X(:,9: 14); 
subplot(223) 

plot(T.Qdot(:,l)*R2D,T,Qdot(:,2)*R2D,T,QdoU:.3)*R2D. ... 

T.Qdot(:,4)*R2D,T,Qdot(:,5)*R2D,T,Qdol(:.(A*R2D); 
titleC ThetaDots  vs  Time'); 

\lahel('Time  (sec)');ylabel(' Angle  Rates  (deg/sec)'), 

"/•Departures  from  Reference  Trajectory 
if  ~BvPass 

subplot(222) 

plot(T,Delq(l,:)*R2D,T,Delq(2,:)*R2D,T,Delq(3,:)*R2D,... 

T,Delq(4,:)*R2D,T,Delq(5,:)*R2D,TX)elq(6,:)*R2D); 
title('Displacement  Errors  vs  Time'); 
xlabel('Time  (sec)');ylabel('Q-QRef  (deg)’); 
end 

if  MetaFlag 
meta  main 
end 

if  DocFlag 
meta  doc  1 
end 
pause 

%  Draw  Motion 
Angles*  Q(:,  1:6); 

|Xcoord,Ycoord|  =  Draw3(Ls,AngConst,Angles,lntenal); 
if  MetaFlag 
meta  main 
end 

if  DocFlag 
meta  doc2 
end 
pause 

dispO; 

disp(’STATE  VECTOR  TIME  HISTORY;'); 
dispCAngles  (deg)') 

Table  1  =  [T  X(:,1:6)*R2D]; 

disp('  Time  ThetaO  ThetaLl  ThetaL2  ThetaRI  ThetaR2  ThctaP'); 

disp(Tablel) 

pause 

disp("); 

disp('Angle  Rates  (deg/sec)') 

Table2  =  [T  Qdot(:,l:6)*R2D]; 

disp('  Time  ThOdot  ThLldot  ThL2dot  ThRldot  ThR2dot  ThPdot'); 

disp(Table2) 

pause 

if  ~ByPass 
disp("); 

disp('TRAJECTORY  ERROR  TIME  HISTORY:'); 
dispCAngles  (deg)') 
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Table2a  =  |T  R2D*Delq(l:6,:)'); 

disp('  Time  DelThO  DelThLl  DelThL2  OelThR  I  DelThR2  DelThP’); 
disp(Table2a) 
end 
pause 

%  Reference  Torque  Histories 
clg; 

if  TfR  >0 
if  TfR  <  TfC 

|r.s|  =  size(  TorqRef  V, 

TorqRef  =  [TorqRef  zeros*  s,  1 )]; 

TRef  =  |T(l:r).  TfR], 
else 

TRef  =  T; 
end 

subplot(221) 
plot(TRef.TorqRef); 
titlc('Reference  Torques  vs  Time'), 
xlabel('Time  ( sec )'), ylabelf  Reference  Torques'), 
end 

%  Command  Torque  Histories 
%Torq  =  (Torq,  zeros(4, 1 )); 
k=n, 

subplot(223) 
plot(T(l:k)',Torq); 
title('Command  Torques  vs  Time'); 
xlabel('Time  (sec)’);ylabel('Command  Torques'). 

%  Cost  Function 
subplol(222) 

plot(T,J(l,:));title(’Cost  vs  Time’); 
s  label  ('Time  (sec)');ylabel('J=abs(Uwh)'); 
subplot(224) 

plot(T,JInt);title(Tntegrated  Cost  vs  Time'); 
xlabelCTime  (sec)');ylabel('JInt’); 
if  MetaFlag 
meta  main 
end 

if  DocFlag 
meta  doc3 
end 
pause 

if  TfR  >  0 
disp(") 

disp('REFERENCE  TORQUE  HISTORY'); 
if  WheelFlag 

disp('  Time  UO  ULS  ULE  ULW  URS  IJRE  URW'); 
else 

disp('  Time  ULS  ULE  ULW  URS  IJRE  URW’); 
end 

Table4  =  [TRef  TorqRef]; 
disp(Table4) 
end 
pause 
disp(") 

disp('COMMAND  TORQUE  HISTORY'); 
if  WheelFlag 

disp('  Time  UO  ULS  ULE  ULW  URS  URE  URW'); 
else 
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dispC  Time  ULS  ULE  ULW  HRS  URE  IJRWV, 
end 

Table5  =  [T(l:k)  Torq'], 

disp(Table5) 

pause 

Table6  =  [T(l:k)  J(l.:)*  JlntJ; 
disp("); 

disp('COST  FUNCTION  HISTORY’), 
disp('  Time  J  JlntAbs  JIntSqr'); 
disp(Table6); 
pause 


%  Angular  Momentum 
k  =  length(T); 
tor  n  =  Ik 

[Hs|  =  AngMo(Ls,Ms,CMs,Is,X(n,  1 :8),X(n,9: 16)), 
if  n  ==  1 

HI  list  =  Hs; 

else 

HHist  =  [HHist;  Hs); 
end 
end 
elg 

subplot(22 1 ); 

plot(T,HHist(:,  1  :6));title('Angular  Momentum  of  Pieces  vs  Time'); 
xlabeK'Time  (sec)');ylabel('Ang  Momentum  (N-m-sec)'); 
subplot(223); 

plot(T,HHist(.,7));title('Total  Angular  Momentum  vs  Time'); 
xlabeK'Time  (sec)');ylabel(’Ang  Momentum  (N-m-sec)'); 

%  Kinetic  Energy 
for  m  =  l:k 
if  AMatFlag 

[M,G,A,Adot,B]  =  MatxFix(Ls,Ms,CMs,Is,X(m,l  ;8),X(m,9;  16),AngConst); 

else 

[M,G,A,Adot,B]  =  Matx(Ls,Ms,CMs,Is,X(m,  1 :8),X(m,9: 16),AngConst); 
end 

I.HSTot(m)  =  0; 

RHSTot(m)  =  0; 

ResTot(m)  =  0; 
for  n=l:8 

LHSTot(m)  =  LHSTot(m)  +  LHS(n,m); 

RHSTot(m)  =  RHSTot(m)  +  RHS(n,m); 
end 

ResTot(m)  =  LHSTot(m)  -  RHSTot(m); 

KE(m)  =  0.5*X(m,9: 16)*M*X(m,9: 16)'; 
end 

subplot(224) 

plot(TTKE);title('Kinetic  Energy  vs  Time’); 
xlabeK'Time  (sec)');ylabelCKE  (kg  mA2/sA2)'); 

%  Compare  wheel  torque  to  change  in  total  angular  momentum 

(Id  =  J(2,:)'; 

subplot(222) 

plot(T(  1  :k)’,Torq(l,:),T(  1  ;k)'JTd); 

title('Compare  Wheel  Torque  to  Change  in  Ang.  Mom.'); 

xlabeK'Time  (sec)'); 

pause 

if  MetaFlag 
meta  main 
end 

if  DocFlag 
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met  a  doc4 
end 

%pause 

elg 

suhplot(221) 

plot(  T.Res);lit  le(  'Residuals  of  liquations'). 

\lal>cl('Time  (see)');ylabel('LI  1S-R1  IS'); 
subplot(223) 

plot(T,Re8Tot),title(Total  Residuals'). 
\label(’Time(sec)');ylabel('LHS-RHS'); 

%  Constraints:  see  if  A*Qdot  =  0  is  satisfied 
subplot(222) 

plot(  l(  1 : k ),Aqdot( I .:),!( 1  :k),Aqdot(2, : ),T(  I  k ). Aqdott ).  ), 
T(I:k),Aqdot(4,:)); 
jdum  1  ,dum2|  =  size(Aqdot); 
ifduml  ==5 
hold  on 

plot(T(  1  :k),Aqdot(5,:)); 
hold  off 
end 

iitle('Constraints:  A*Qdot  vs  Time'); 

\label('Timc  (sec)');ylabel('A*Qdot'); 
if  Metal-Tag 
meta  main 
end 

if  DocFlag 
meta  doc5 
end 
pause 

end  %  End  of  OutFlag  branch 


I.  Matx 

%  Filename  is  'Matx.m' 

%  This  routine  calculates  the  matrices  for  the  dual  arm 
%  spacecraft  EOM  when  it  is  grasping  a  payload.  Each  arm 
%  has  two  links.  This  version  assumes  that  the  ccntcrbody 
%  is  NOT  fixed.  This  impacts  A  and  Adot. 

function  [M,G,A-Adot.B]  =  Matx(Ls,Ms,CMs,ls,Ths,Thdots,AngConst) 

%  OUTPUTS: 

%  M  =  8x8  mass  matrix 

%  G  =  8x  1  vector  with  coriolis  and  centripetal  terms 
%  A  =  4x8  constraints  matrix 
%  Adot  =  4x8  derivative  of  constraints  matrix 
%  B  =  Control  influence  matrix 
% 

%  INPUTS: 

%  Ls  -  7x  1  vector  of  lengths  (m) 

%  1  st  e  nent  =  distance  from  origin  to  left  arm  mount 

%  2nd  3rd  elements  wrt  left  arm  (from  shoulder  to  wrist) 

%  4th  element  =  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  to  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  [LO;Ll;L2;LP;R2;Rl;RO] 

%  Ms  =  6x  1  column  vector  containing  the  masses  (kg) 
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%  1  st  dement  =  mass  of  spacecraft  centerbod  v 

%  2nd  &  3rd  elements  =  left  arm  (upper  then  lower  arm) 

%  4th  &  5th  elements  =  right  arm  (upper  then  lower  arm) 

%  6th  element  =  payload  mass 
%  IMO,  ML  1 .  ML2,  MR  1 .  MR2,  MP] 

%  CMs  =  6x  I  column  vector  containing  center  of  mass  locations  <  m ) 
%  [LcO;  LcLl;  LcL2;  LcRl  .  LcR2,  LeP| 

%  Is  =  6x  1  column  vector  containing  the  moments  of  inertias 
%  about  the  respective  body's  center  of  mass  t  kg  mA2) 

%  1  si  element  =  inertia  of  spacecraft  eenterbody 

%  2nd  &  3rd  elements  =  left  arm  (upper  then  lower  arm ) 

%  4th  &  5th  elements  =  right  arm  (upper  then  lower  arm ) 

%  6th  element  =  payload  inertia 
%  110;  IL 1 ;  1L2;  1R1;  1R2;  IP) 

%  Ths  =  6  element  vector  containing  the  angles  which  describe 
%  the  configuration  of  the  system. 

%  IThO;  ThL  1 ;  ThL2,  ThR  1 ;  ThR2,  ThPl 
%  I  hdots  =  6  element  vector  containing  the  angle  rates 
%  AngConst  =  2  element  vector  of  arm  mounting  locations 
%  [ThLO;  ThRO] 

%%%%%%%%%0/o%%%%%%%%%%%%%%%^.%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

1.0  =  Ls(l); 

LI  =  Ls(2); 

1.2  =  Ls(3); 

LP  =  Ls(4); 

R2  =  Ls(5); 

R I  =  Ls(6); 

RO  =  l.s(7); 

%  Member  masses  (kg) 

MO  =  Ms(l); 

ML  I  =  Ms(2); 

ML2  =  Ms(3); 

MR1  =Ms(4); 

MR2  =  Ms(5); 

MP  =  Ms(6); 

%  Center  of  mass  distances  (m) 

LeO  =  CMsfl); 

LcLl  =  CMs(2); 

LcL2  =  CMs(3); 

LcRI  =  CMs(4); 

LcR2  =  CMs(5); 

LcP  =CMs(6);  %measured  from  left  end 

%  MOI  about  center  of  mass 
10  =Is(l). 

IL  1  =  Is(2); 

IL2  =  Is(3); 

IR 1  =  Is(4); 

IR2  =  Is(5); 

IP  =  Is(6); 

%  Angles 
ThO  =  Ths(l); 

ThLl  =  Ths(2); 

ThL2  =  Ths(3); 


thRl  =  Ths(4); 
ThR2  =  Ths(5), 
IhP  =  Ths(6), 


%  Angle  Rates 
i  h()d  =  Thdots(  1 ); 
ThLld  »  Thdots(2); 
ThL2d  *  Thdots(3); 
ThRld  =  Thdots(4); 
ThR2d  =  Thdots(5). 
I'hPd  =  Thdols(6); 

%  Arm  mount  locations 
Thl.O  =  AngConst(l); 
1'hRO  =  AngConst(2), 


% %%%%%%%%%% 

%%  Mass  Matrix  %% 

%%%%%%%%%%% 

M  =  zeros(8,8); 

M(8,8)  =  MP, 

Mf7,7)  =  MP; 

M(6,6)  =  IP; 

M(5,5)  =  1R2  +  MR2*LcR2A2. 

M(5,4)  =  M(5,5)  +  MR2*Rl*LcR2*cos(ThR2); 

M(4,5)  =  M(5,4); 

M(5, 1 )  =  M(4,5)  +  MR2*RO*LcR2*cos(ThR  1  +ThR2); 

M(  1,5)  =  M(5,l); 

M(  4,4)  =  M(4,5)+IR  1  +MR2*R  1  *LcR2*cos(TliR2)+MR  1  *l.cR  1  A2+MR2*R  1 A2; 
M(4, 1  )=M(4,4)+R0*(MR  l*LcR  1  +MR2*R  1  )*cos(  I  hR  1  )+MR2*R0*I .cR2* .. . 

cos(ThRl+ThR2); 

M(l,4)  =  M(4,l); 

M(3,3)  =  1L2  +  ML2*LcL2A2; 

M(3,2)  *  M(3,3)  +  ML2*Ll*LcL2*cos(ThL2); 

M(2,3)  =  M(3,2); 

M(3,l)  =  M(3,2)  +  ML2*L0*LcL2*cos(ThL  l+ThL2). 

M(l,3)  =  M(3,l); 

M(2,2)  =  M(3,2)+ML2*L  1  *LcL2*cos(ThL2HIL  1 +ML 1  *I,cL  1  A2+ML2*L  I A2; 
M(2, 1  )=M(2,2)+L0*(ML  1  *LcL  1  +ML2*L  1  )*cos(ThL  1  )+ML2*Id)*l  xl.2*. . . 

cos(ThL  1  +ThL2); 

M(l,2)  =  M(2,l); 

Parti  =  I0+M(2,2)+M(4,4)+M0*Lc0A2+(MU+ML2)*i,0A2+(MR  l+MR2)*ROA2; 
Part2  =  2*LO*(MLl*LcLl+ML2*Ll)*cos(ThLl)+2*MI,2*LO*I.cl.2*... 
cos(ThLl+ThL2); 

Part3  =  2*RO*(MRl*LcRl+MR2*Rl)*cos(ThRl>+2*MR2*RO*LcR2* ... 

cos(ThRl+ThR2); 

M(l,l)  =  Parti  +  Part2  +  Part3; 


%%%%%% 

%%  G  %% 

%%%%%% 

G  =  zeros(8>l); 

Pt  1  =  -L0*(ThL  1  dA2+2*Th0d*ThL  1  d)*(ML  1  *LcL  1  +ML2*L  1  )*sin(ThL  1 ); 
Pt2  =  -ML2*L  1  *LcL2*ThL2d*(2*ThOd+2*ThLld+ThL2d)*sin(ThI,2), 
Pt3=-ML2*LO*LcL2*(2*ThOd#(ThL  1  d+ThL2d)+(Thl ,  1  d+ThL2d)A2)*  . . 
sin(ThLl+ThL2); 

Pt4  =  -R0*(ThR  1  dA2+2*Th0d*ThR  ld)*(MR  1  *LcR  1  +MR2*R  1  )*sm(ThR  I ); 
Pt5  =  -MR2*Rl*LcR2*ThR2d*(2*Th0d+2*ThRld+ThR2d)*sin(ThR2); 
Pt6=-MR2*R0*LcR2*(2*Th0d*(ThRld+ThR2d)+(ThRId+ThR2d)A2)*.. 
sin(ThRl+ThR2); 

G(  I )  =  Pt  I  +  Pt2  +  Pt3  +  Pt4  +  Pl5  +  Pt6; 
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Pt  I  =  LO*ThOdA2*(ML  1  *LcLl  +ML2*L  I  )*sin(  Till .  1 ). 

P12  =  -ML2*Ll*LcL2*ThL2d*(2*ThOd+2*ThLld+ThL2d)*sin(ThL2). 
Pt3  =  ML2*LO*LcL2*ThOdA2*sm(ThL  l+ThL2); 

0(2)  =  Pt  l  +  Pt2  +  Pt3; 

(i(3)  =  ML2*LcL2*(Cl*(ThOd+ThLld)A2*sin(Thl.2)+I.O*TliOdA2* 
sindhl.  i+ThL2)), 

Pt  I  =  R0*Th0d A2*( M R  l  *LcR 1  +MR2*R  1  )*sm(  IhR  I ). 

Pt2  =  -MR2*R  1  *LcR2*ThR2d*(2*ThOd+2*ThRld+ThR2d)*sin(ThR2); 
Pt3  =  MR2*RO*LcR2*ThOdA2*sin(ThR  1  +ThR2 ). 

C(4)  =  Ptl  +  Pt2  +  Pt3; 

0(5)  =  MR2*L.cR2*(R  1  *(ThOd+ThR  I  d)A2*sin(  ThR2)+RO*ThOdA2* 
sm(ThRl+ThR2)), 


%%%%%%%%%%%%%% 

%%  Constraints  Matrix  %% 

%%%%%%%%%%%%%% 

%  The  constraint  matrix  comes  from  putting  the  constraint  equations 
%  into  the  Pfaffian  form:  A*qdot  +  AO  =  0.  The  first  two  constraint 
%  equations  are  found  by  finding  the  x  and  v  components  of  the 
%  Payload's  center  of  mass  by  starting  at  the  origin  and  moving 
%  up  the  left  arm.  The  second  two  constraint  equations  find  the  x 
%  and  y  components  of  the  Payload's  center  of  mass  by  starting  at  the 
%  origin  and  moving  to  the  base  of  the  right  arm  and  then 
%  up  the  right  arm.  Differentiating  these  equations  results 
%  in  the  Pfaffian  form  with  AO  =  0. 

A  =  zeros(4,8); 

A(  1 ,7)  =  - 1 ; 

A(2,8)  =  -  I; 

A(3,7)  =  -!; 

A(4,8)  =  - 1 ; 

A(  1,6)  =  -I,cP*sin(ThP); 

A(2,6)  =  LcP*cos(ThP); 

A(3,6)  =  (l,P-LcP)*sin(ThP); 

A(4,6)  =  -(LP-LcP)*cos(ThP); 

A(4,5)  =  R2*cos(ThO+ThR(HThR  1  +ThR2); 

A(4,4)  =  A(4,5)  +  Rl*cos(ThO+ThRO+ThRi). 

A(4, 1 )  =  A(4,4)  +  RO*cos(ThO+ThRO). 

A(3,5)  =  -R2*sin(ThO+ThRO+ThR  1  +ThR2), 

A(3,4)  =  A(3,5)  -  R 1  *sin(ThO+ThRO+ThR  I ), 

A(3,l)  =  A(3,4)  -  RO*sin(ThO+ThRO); 

A(2,3)  =  L2*cos(ThO+ThLO+ThL  l+ThL2); 

A(2,2)  =  A(2,3)  +  L 1  *cos(ThO+ThLO+ThL  1 ); 

A(2,l)  =  A(2,2)  +  I.O*cos(ThO+ThLO); 

A(  1 ,3)  =  -L2*sin(ThO+ThLO+ThL  l+ThL2). 

A(  1 ,2)  =  A(  1 ,3)  -  L 1  *sin(ThOt-ThLO+ThL  1 ); 

A(  1 , 1 )  *  A(  1 ,2)  -  LO*sin(ThO+ThLO); 


Adot  =  zeros(4,8), 

Adot(  1 ,6) « -ThPd*LcP*cos(ThP); 

Adot(2,6)  =  -ThPd*LcP*sin(ThP); 

Adot(3,6)  =  ThPd*(LP-LcP)*cos(ThP); 

Adot(4,6)  =  ThPd*(LP-LcP)*sin(ThP); 

Adot(4,5)  =  -(ThOd+ThR  ld+ThR2d)*R2*sin(ThO+ThRO+ThR  I  +ThR2); 
Adot(4,4)  =  Adot(4,5)  -  (ThOd+ThR  1  d)*R  1  ♦sinf  ThO+ThRO+ThR  I ); 
Adot(4,l)  =  Adot(4,4)  -  ThOd*RO*sin(ThO+ThRO); 

Adot(3,5)  =  -(ThOd+ThR  ld+ThR2d)*R2*cos(Th()+ThRO+ThR  1  +ThR2); 
Adot(3,4)  =  Adot(3,5)  -  (ThOd+ThR ld)*R  1  *cos( ThO+TliRO+  ThR  1 ), 
Adot(3,l)  =  Adot(3,4)  -  ThOd*RO*cos(ThO+ThRO); 

Adot(2,3)  =  -(ThOd+ThL  ld+ThL2d)*L2*sin(ThO+ThLO+ThL  1  +ThL2); 
Adot(2,2)  =  Adot(2,3)  -  (ThOd+ThL  1  d)*L  1  *sin(ThO+ThLO+ThL  1 ), 
Adot(2,l)  =  Adot(2,2)  -  ThOd*LO*sin(ThO+ThLO); 
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Adot( 1 ,3)  =  -(ThQd+Thl.  Id+ThL2d)*L2*costThO+ Till O+Thl.  1  +  l'hl.2); 
Adot(  1 ,2)  =  Adol(  1,3)-  (ThOd+ThL  ld)*L  1  *cos(Th()+  I  hLO+Thl.  1 ): 
Ad«t(  1,1)  =  Adot(l,2)  -  ThOd*LO*cos(ThO+ThLO). 


%%%%%% 
%%  13  %% 
%%%%%% 

13  =  7.eros(8,6). 
13(1,3)  =  -1 . 
13(1,6)  =  -1, 
13(2,1)=  1, 
13(2,3)= -1; 
13(3.2)=  I; 
13(3.3)  = -1. 
13(4,4)=  1; 
13(4,6)  = -1; 
13(5,5)=  1, 
13(5.6)  =  -l; 
13(6,3)=  1, 
13(6,6)=  1; 


J.  MatxFix 

%  Filename  is  'MatxFix.m' 

%  This  routine  calculates  the  matrices  for  the  dual  arm 
%  spacecraft  EOM  when  it  is  grasping  a  payload.  Each  arm 
%  has  two  links.  This  version  assumes  that  the  cenlerbodv 
%  is  fixed.  This  impacts  A  and  Adot. 

function  [M,G,A,Adot,B[  =  Matx(Ls,Ms,CMs,Is.Ths.Thdots.AngConst) 

%  OUTPUTS: 

%  M  =  8x8  mass  matrix 

%  G  =  8x  1  vector  with  coriolis  and  centripetal  terms 
%  A  =  5x8  constraints  matrix 
%  Adot  =  5x8  derivative  of  constraints  matrix 
%  13  =  Control  influence  matrix 

% 

%  INPUTS: 

%  I.s  =  7\1  vector  of  lengths  (m) 

%  1  st  element  =  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  to  wrist) 

%  4th  element  =  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  to  shoulder) 

%  7  th  element  =  distance  from  right  arm  mount  to  on  gin 

%  [LO;  LI;  L2;  LP;  R2;  Rl;  ROJ 
%  Ms  =  6x  1  column  vector  containing  the  masses  (kg) 

%  1  st  element  =  mass  of  spacecraft  centerbodv 

%  2nd  &  3rd  elements  =  left  arm  (upper  then  lower  arm ) 

%  4th  &  5th  elements  =  right  arm  (upper  then  lower  arm ) 

%  6th  element  =  payload  mass 
%  [MO;  ML  1 ;  ML2;  MR  1 ;  MR2,  MP) 

%  CMs  =  6x  1  column  vector  containing  center  of  mass  locations  (m) 

%  [LcO;  LcLl;  LcL2;  LcRl;  LcR2;  LcP| 

%  Is  =  6x  1  column  vector  containing  the  moments  of  inertias 
%  about  the  respective  body's  center  of  mass  ( kg  mA2) 

%  1  st  element  =  inertia  of  spacecraft  cenlerbodv 

%  2nd  &  3rd  elements  =  left  arm  (upper  then  lower  arm) 

%  4th  &  5th  elements  =  right  arm  (upper  then  lower  arm) 


146 


%  6lh  element  =  payload  inertia 
%  [10;  IL1;  1L2;  IR1;  IR2;  IPj 

%  Ths  =  6  element  vector  containing  the  angles  which  describe 
%  the  configuration  of  the  system 
%  [ThO,  ThL  1 ;  ThL2,  ThRl.  ThR2:  ThP| 

%  Thdots  =  6  element  vector  containing  the  angle  rates 
%  AngConst  =  2  element  vector  of  arm  mounting  locations 
%  [ThLO;  ThROJ 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  Lengths  (m) 

L0  =  Ls(l); 

Li  =  Ls(2), 

L2  =  l.s(3); 

LP  =  Ls(4); 

R2  =  Ls(5); 

R1  =  Ls(6); 

R0  =  Ls(7); 

%  Member  masses  (kg) 

MO  =  Ms(l); 

ML1  =  Ms(2); 

ML2  =  Ms(3); 

MR1  =  Ms(4), 

MR2  =  Ms(5); 

MP  =  Ms(6); 

%  Center  of  mass  distances  (m) 

LcO  =  CMs(  1 ); 

LcLl  =  CMs(2); 

LcL2=CMs  (3); 

LcRl  =CMs(4). 

I. cR2  =  CMs(5); 

LcP  =  CMs(6);  %measured  from  left  end 

%  MOl  about  center  of  mass 
10  =  Is(  1); 

II,  1  =Is(2); 

IL2  =  Is(3), 

IR1  =  Is(4); 

IR2  =  Is(5); 

IP  =  ls(6); 

%  Angles 
ThO  =  Ths(  1 ); 

ThL  1  =  Ths(2); 

ThL2  =  Ths(3); 

ThRl  =Ths(4); 

ThR2  =  Ths(5); 

ThP  =  Ths(6); 

%  Angle  Rates 
ThOd  =  Thdotsfl), 

ThL  Id  =  Thdots(2); 

Thl,2d  =  Thdots(3); 

ThRld  =  Thdots(4); 

ThR2d  =  Thdots(5); 

ThPd  =Thdots(6); 
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%  Arm  mount  locations 
ThI.O  =  AngConst(  1 ), 

ThRO  =  AngConsl(2), 

"<>%%%%%%%%%% 

%%  Mass  Matrix  %% 

%%%%%%%%%%% 

M  =  zeros(8,8); 

M(8,8)  =  MP. 

M(7,7)  =  MP. 

M<6.6)=  IP; 

M(5,5)  =  1R2  +  MR2*LcR2A2, 

M(5,4)  =  M(5,5)  +  MR2*Rl*LcR2*cos(ThR2V. 

M(4,5)  =  M(5,4); 

M(5.1)  =  M(4,5)  +  MR2*RO*LcR2*cos(ThR  1  +ThR2); 

M(l,5)=  M(5,l); 

M(4,4)  =  M(4,5)+IR  1  +MR2*R  1  *LcR2*cos<  TliR2)+MR  1  *1  .cR  1  A2+MR2»R  1 A2; 
M(4, 1  )=M(4,4)+R0*(MR  1  *LcR  1  +MR2*R  1  )*cos(ThR  1  )+MR2*R< >*I.cR2*  . 

cos(ThR  1  +ThR2); 

M(l,4)  =  M(4,l); 

M(3,3)  =  1L2  +  ML2*LcL2A2; 

M(3,2)  =  M(3,3)  +  ML2»L  1  *LcL2*cos(ThL2); 

M(2,3)  =  M(3,2); 

M{3.1)  =  M(3,2)  +  ML2*LO*Lcl.2*cos(TliI.  i+ThL2), 

M(l,3)  =  M(3,l); 

M(2,2)  =  M(3,2)+ML2*L  l*LcL2*cos(ThL2)+IL  1 +ML I  *LcL  1  A2+MI.2*L  1 A2; 
M(2,l  )=M(2.2)+L0*(ML  I  *LcL  1  +ML2*L  1  )*cos(ThL  1  )+ML2*I.O*I.cL2*. 

cos(ThL  1  +ThL2); 

M(  1,2)  =  M(2,l); 

Parti  =  10+M(2,2)+M(4,4>+M0*Lc0A2+(MI,l+M1.2)*l,0A2+(MRl+MR2)*R0A2; 
Part2  =  2*LO*(MLl*LcLl+ML2*Ll)*cos(ThlJ)+2*ML2*LO*I.cL2* ... 
cos(ThLl+ThL2); 

Part3  =  2*R0*(MR  1  *LcR  1+MR2*R  1  )*cos(ThR  1  )+2*MR2*RO*LcR2*. 

cos(ThR  1  +ThR2); 

M(l.l)  =  Parti  +  Part2  +  Part3; 

%%%%%% 

%%  G  %% 

%%%%%% 

G  =  zeros(8,l); 

Pt  1  =  -L0*(ThL  1  dA2+2*Th0d*ThL  1  d)*(ML  1  *LcL  1+ML2*LI  )*sin(ThL  1 ); 

Pt2  =  ~ML2*L  1  *LcL2*ThL2d*(2*ThOd+2*ThL  1  d+Thl.2d)*sin(ThI .  2); 
Pt3=-ML2*L0*LcL2*(2*Th0d*(ThL  ld+ThL2d)+(TliLld+ThI.2d)A2)* ... 
sin(ThLl+ThL2); 

Pt4  =  -RO*f DiR  1  dA2+2*Th0d*ThR  1  d)*(MR  1  *1  .cR  1  +MR2*R  1  )*sin(ThR  1 ); 

Pt5  =  -MR2*Rl*LcR2*ThR2d*(2*Th0d+2*ThRld+ThR2d)*sin(ThR2); 
Pt6=-MR2*R0*LcR2*(2*Th0d*(ThR  1  d+ThR2d)+(ThR  1  d+ThR2d)A2)*. . . 
sin(ThRl+ThR2); 

G(l)  =  Ptl  +  Pt2  +  Pt3  +  Pt4  +  Pt5  +  Pt6; 

Pt  1  =  LO*ThOdA2*(ML  1  *LcL  1  +ML2*L  1  )*sin(ThI  - 1 ); 

Pt2  =  ~ML2*Ll*LcL2*ThL2d*(2*ThOd+2*ThLld+ThL2d)*sin(ThL2); 

Pt3  =  ML2*LO*LcL2*ThOdA2*sin(ThLl+ThL2); 

G(2)  =  Ptl  +P12  +  PG; 

G(3)  =  ML2*LcL2*(L  1  *(ThOd+ThL  1  d)A2*sin(ThL2)+LO*ThOdA2*. . . 
sin(ThL  1  +ThL2)); 

Pt  1  =  RO*ThOdA2*(MR  1  *LcRl  +MR2*R  I  )*sin(ThR  1 ); 

Pt2  =  -MR2*Rl*LcR2*ThR2d*(2*ThOd+2*ThRld+TliR2d)*sin(  lliR2); 

Pt3  =  MR2*RO*LcR2*ThOdA2*sin(  ThR  1  +ThR2); 

G(4)  =  Ptl  +  Pt2  +  Pt3; 

0(5)  =  MR2*LcR2*(Rl*(ThOd+ThRld)A2*sm(ThR2)+RO*ThOdA2*.. 
sin(ThRl+ThR2)); 
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%%•/.%%%%%%%%%%•/. 

%%  Constraints  Matrix  %% 

%%%%%%%%%%%%%% 

%  The  constraint  matrix  comes  from  putting  the  constraint  ec|uations 
%  into  the  Pfafilan  form:  A‘qdot  +  AO  =  u  I  he  first  two  constraint 
%  equations  are  found  by  finding  the  x  and  v  components  of  the 
%  Payload's  center  of  mass  by  starting  at  the  origin  and  moving 
%  up  the  left  arm.  The  second  two  constraint  equations  find  the  x 
%  and  v  components  of  the  Payload's  center  of  mass  by  starling  at  the 
%  origin  and  moving  to  the  base  of  the  right  arm  and  then 
%  up  the  right  arm  Differentiating  these  equations  results 
%  in  the  Plaffian  form  with  AO  =  0 
A  =  zeros(S,8); 

A(5,l)  =  1. 

A(  1,7)  =  -  I; 

A(2,8)  =  -1 , 

A(3,7)  =  -1; 

A(4,8)  =  -  l; 

A(  1 ,6)  =  -LcP‘sin(ThP); 

A(2,6)  =  LcP*cos(ThP); 

A(3,6)  =  (!.P-I.cP)*sin(ThP|, 

A(4,6)  =  -(LP-LcP)*cos(ThP), 

A(4,5)  =  R2*cos(ThO+ThR(HThRI+ThR2). 

A(4,4)  =  A(4,5)  +  R I  *cos(ThO+ThRO+ThR  I ); 

A(4, 1 )  =  A(4,4)  +  RO*cos(ThO+ThRO), 

A(3,5)  =  -R2*sin(ThO+ThRO+ThR  I  +ThR2); 

A(3,4)  =  A(3,5)  -  R 1  ‘sin(ThO+ThRO+ThR  1 ); 

A(3,l)  =  A (3,4)  -  RO*sin(ThO+ThRO), 

A(2,3)  =  L2*cos(  IhO+ThLO+ThL  1  +ThL2); 

A(2,2)  =  A(2,3)  +  L 1  ‘cost  ThO+ThLO+ThL  I ); 

A(2,l)  =  A(2,2)  +  LO*cost  BiO+ThLO), 

A(  1 ,3)  =  -L2‘sin(ThO+ThLO+ThL  1  +TLL2). 

A(  1 ,2)  =  A(  1 ,3)  -  L 1  *sin(ThO+ThLO+ThL  1); 

A(  1 , 1 )  =  A(  1 ,2)  -  LO‘sin(ThO+ThLO); 

Adot  =  zeros(5,8), 

Adot(  1 ,6)  =  -ThPd*LcP‘cos(ThP); 

Adot(2,6)  =  -ThPd‘LcP*sin(ThP); 

Adot(3,6)  *  ThPd‘(LP-LcP)*cos(ThP); 

Adot(4,6)  =  ThPd‘(LP-LcP)*sin(ThP), 

Adot(4,5)  =  -(ThOd+ThR|d+ThR2d)*R2*sin(ThO+ThRO+ThRl+ThR2); 
Adot(4,4)  =  Adot(4,5)  -  (ThOd+ThRld)‘R  1  *sin(TliO+ThRO+ThR  I ); 
Adot(4, 1 )  =  Adot(4,4)  -  ThOd‘RO*sin(ThO+ThRO); 

Adot(3,5)  =  -(ThOd+ThRld+ThR2d)*R2*cos(ThO+ThRO+ThR  I  +ThR2). 
Adot(3,4)  =  Adot(3,5)  -  (ThOd+ThRld)*Rl*cos(ThO+ThRO+ThRl), 
Adot(3,l)  ~  Adot(3,4)  -  Th0d*R0*cos(Th0+ThR0). 

Adot(2,3)  =  -(Th0d+ThLld+ThL2d)*L2‘sin(Th0+ThI,0+ThI,l+nil,2); 
Adot(2,2)  =  Adot(2,3)  -  (ThOd+ThL  ld)*L  I  ‘sin(ThO+ThLO+  ITiL  I ): 
Adot(2,l)  =  Adot(2,2)  -  ThOd*LO*sin(ThO+ThLO); 

Adot(l,3)  =  -(ThOd+ThL  1  d+ThL2d)*L2*cos(  ThO+Tht.O+ThL  I +ThL2); 
Adot(l,2)  =  Adot(l,3)  -  (ThOd+ThL  1  d)*L  1  *cos(ThO+ThLO+ThL  1 ), 
Adot(  1,1)  =  Adot(  1 ,2)  -  Th0d*L0‘cos(Th(>+ThL0); 

%%%%%% 

%%  B  %% 

%%%%%% 

B  =  zeros(8,6); 

B(l,3)  =  -1; 

B(  1 ,6)  =  - 1 ; 

B(2,l)  =  1; 
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11(2.31  =  - 
11(3,2)  =  i 
11(3,3)  =  - 
11(4.4)  = 
11(4,6)=  - 
11(5.5)  = 
11(5.6)  =  - 
11(6.3)=  I 
11(6,6)=  ! 


K.  Ref2 

%  Filename  is  ’Ref2.m' 

%  Reference  Maneuver  using  cosl  function 
%  This  routine  assumes  that  the  “pacecralt  centerbody  is  held  fixed 

function  |Torques,QRef,QdotRef.Aqdot,J,Cl,C2,C3|  = 

Ref2(Ls,Ms,CMs,ls,UoundC,T,Wu,Wc,Coef,ConstMat) 

%  OUTPUTS: 

%  Torques  =  7x1  column  vector  of  torques  that  should  he  applied  at 
%  time  T  if  the  motion  is  to  follow  the  reference  trajectory 

%  exactly.  The  vector  is  arranged  as  |U().  IJ1.S.  UU-.  ULW.  IJRS:  URL.  URW| 

%  which  are  the  centerbody  torque  f  ollowed  by  the  torques  at  the 

%  shoulder,  elbow,  and  wrist  of  the  left  arm  and  then  the  right  arm 

%  respectively 

%  QRef  =  8x  1  column  vector  of  reference  generalized  coordinates 
%  OdotRcf  =  8x1  column  vector  of  reference  generalized  velocities 
%  Aqdot  =  4x1  or  5x  1  column  vector  (depends  on  status  of  AMatFlag )  which 
%  check  to  see  if  the  constraint  equation  A*Qdot  =  0  is  satisfied 
%  J  =  scalar  value  of  the  reaction  wheel  torque  absolute  value  This 
%  number  will  be  integrated  to  find  the  value  for  the  cost  function 
%  Lyapunov  Controller  matrices  (reference  trajectory  values) 

%  C 1  =  8x7  matrix 

%  C2  =  8x4  or  8x5  (depends  on  status  of  AMatFlag)  matrix 
%  C3  =  8x1  matrix 

% 

%  INPUTS: 

%!.s  =  7xl  vector  of  lengths  (m) 

%  I  st  element  =  distance  from  origin  to  left  arm  mount 

"-cl  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  toward  wrist) 

%  4th  dement  =  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  toward  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  ILO;  L 1 ;  L2,  LP,  R2;  R 1 ,  R0| 

%  Ms  =  6x1  column  vector  containing  the  masses  (kg) 

%  I  st  element  =  mass  of  spacecraft  centerbody 

%  2nd  &  3rd  elements  =  mass  of  left  arm  (upper  arm  then  lower  arm) 

%  4th  &  5th  elements  =  mass  of  right  arm  (upper  arm  then  lower  arm) 

%  6th  element  =  payload  mass 
%  [MO;  ML1,  ML 2,  MR1;  MR2;  MP| 

%  CMs  =  6x1  column  vector  containing  center  of  mass  locations 
%  [LcO;  LcL  I ;  LcL2;  LcR  1 ;  LcR2,  LcP  | 

%  Is  =  6x  1  column  vector  containing  the  moments  of  inertias  about  the 
%  respective  body's  center  of  mass  (kg  mA2) 

%  1  st  element  =  inertia  of  spacecraft  centerbody 

%  2nd  &  3rd  elements  =  inertia  of  left  arm  (upper  arm  then  lower  arm) 

%  4th  &  5th  elements  =  inertia  of  right  arm  (upper  arm  then  lower  arm) 

%  6th  element  =  payload  inertia 
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%  IK),  IL1.1L2.  IR1;  IR2;  IP] 

%  BoundC  =  boundry  conditions  for  the  problem  The  first  column 
‘‘"ti  contains  the  initial  x  and  v  component  of  points  Q  &  P 

%  respectively,  the  x  component  of  the  right  ann  base,  the 

%  problem  start  time,  and  the  simulation  stop  time  The  second 
"-li  column  contains  the  x  and  v  component  of  points  Q  &  P 

%  respectively,  the  x  component  of  the  rigid  arm  base,  the 

%  stop  time  for  the  ideal  reference  maneuver,  and  a  (lag  to 
%  activate  or  deactivate  the  controller  The  origin  for  the 
"'ll  x  and  v  components  is  the  base  of  the  left  arm. 

%  T  =  time 

"li  Wu  =  6x6  or  7x7  control  torque  cost  weighting  matrix 

%  Wc  =  8x8  constraint  cost  weighting  matrix 

"'ll  Cocf  =  (n-2)xl  column  vector  of  reference  polynomial  coefficients 

"'ll  beginning  with  order  n  coefficient 

%  ConstMat  =  3x(n-2)  matrix  of  coefficients  for  reference  displacement 
%  (row  1 ),  velocity  (row  2),  and  acceleration  (row  3) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

1.0  =  Ls(l); 

1.1  =  Ls(2); 

L2  =Ls(3); 

I.P  =  Ls(4); 

R2  =  Ls(5); 

R1  =  Ls(6); 

RO  =  Ls(7); 

%  Member  masses  (kg) 

MO  =  Ms(l); 

ML1  =  Ms(2); 

ML2  =  Ms(3); 

MR1  =  Ms(4); 

MR2  =  Ms(5), 

MP  =  Ms(6); 

%  Center  of  mass  distances  (m) 

Lc()  =  CMs(l); 

LcLl  =  CMs(2); 

LcL2  =  CMs(3), 
l.cRl  =  CMs(4); 

LcR2  =  CMs(5); 

LcP  =  CMs(6);  %measured  from  left  end 

%  MOl  about  center  of  mass 
10  =  Is(  1 ); 

ILI  =  Is(2); 

IL2  =  Is(3); 

1R1  =Is(4); 

IR2  =  Is(5); 

IP  =  Is(6); 

%  Initial  and  final  locations  of  third  link 
%  Point  Q  is  at  Node  3  (joint  between  Links  2  &  3) 

%  Point  P  is  at  Node  4  (joint  between  Links  3  &  4) 

QxO  =  BoundC ( 1,1);  QyO  =  BoundCf  1 ,2); 

PxO  =  BoundC(2,l);  PyO  =  BoundC(2,2); 

Qxf  =  BoundC(3,l);  Qyf  =  BoundC(3,2); 

Pxf  =  BoundC(4, 1 );  Py  f  =  BoundC(4,2); 
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%  Arms  mount  locations  wrt  spacecraft  centerbodv  coordinate  frame  (rad) 
ThLO  =  BoundC(5, 1 );  ThRO  =  BoundC<5,2) 

%  Reference  Maneuver  Start  and  Stop  l  imes 
TO  =  BoundC(6,l);  Tf  =  BoundC(6,2); 

%  Constraints  Matrix  Flag 
AMall'lag  =  BoundC(8,l); 

%  Centerbody  Reaction  Wheel  Flag 
Wheel  Flag  =  BoundC(8,2); 

%  Centerbodv  Initial  and  Final  Co  iditions 
IhOO  =  BoundC(9,l); 

ThOf  =  BoundC(9,2); 

%  Number  of  equations  in  the  cost  function  constraint  equations 
HOMFlag  =  BoundC(lO.l); 

%  Psuedo-lnverse  Flag 
PInvFlag  =  BoundC(10,2); 


%%%%%%%%%%%%%%%%%%%%%% 

%%  PRELIMINARY  CALCULATIONS  %% 
%%%%%%%%%%%%%%%%%%%%%% 

R2D  =  1 80/pi;  %  Conversion  from  radians  to  degrees 

%  Total  rotation  of  Payload 

ThPO  =  atan2(PyO-QyO,PxO-QxO);  %  Initial  angle  of  Payload  (rad) 

ThPf  =  atan2(Pyf-Qyf,Pxf-Qxf);  %  Final  angle  of  Payload  (rad) 

DelThP  =  ThPf  -  ThPO;  %  Total  delta  angle  of  Payload  (rad) 

%  Initial  and  final  locations  of  Pavload  center  of  mass 
XPO  =  QxO  +  (PxO  -  QxO)  *  (LcP/LP); 

YPO  =  QyO  +  (PyO  -  QyO)  *  (LcP/LP); 

XPf  =  Qxf  +  (Pxf  -  QxO  *  (LcP/LP); 

YPf  =  Qyf  +  (Pyf  -  QyO  *  (LcP/LP); 

Tau  =  (T-TO)  /  (Tf-TO);  %  Normalize  time 

%  Function  Weighting  Factors  for  how  the  payload  will  move 
%  These  factors  will  cause  the  velocity  and  acceleration  of 
%  the  payload  coordinates  to  be  zero  at  t  =  0  and  t  =  tf. 

%  They  also  permit  the  displacements  for  the  payload  coordinates 
%  to  match  their  initial  and  final  values.  These  weighting 
%  factors  will  also  apply  to  the  centerbody  rotation. 

k  =  length(Coef); 
for  n=l:k 

CTau(k+l-n)  =  Coef(k+i-n)*TauA(n+2); 

CTaud(k+l-n)  =  Coef(k+i  -n)*TauA(n+ 1 ); 

CTaudd(k+l-n)  =  Coef(k+l-n)*TauA(n); 
end 

%  Weighting  factors 
W  =  ConstMat(l,:)*CTau'; 

Wd  =  ConstMat(2,:)*CTaud'; 

Wdd  =  ConstMat(3 , ;  )*CTaudd' ; 

%  Centerbody  angle,  angular  velocity,  angular  acceleration 
DelThO  =  ThOf  -  ThOO; 
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ThO  =  ThOO  +  W  *  DelThO;  %  Angle  (rad). 

ThOd  =  Wd  *  DelThO  /  (Tf  -  TO),  %  Velocity  <  rad/sec ); 

ThOdd  =  Wdd  *  DelThO /(Tf  -  TO)A2;  %  Acceleration  (rad/secA2). 
%ThO  =0; 

%'1'hOd  =0; 

% ThOdd  =  0; 

%  Save  for  plotting 
QRef(l)  =  ThO, 

QdotRel(l)  =  ThOd. 

QddotRef(  1 )  =  ThOdd; 

%  Pavload  angle,  angular  velocity,  angular  acceleration 
ThP  =  ThPO  +  W  *  DelThP,  %  Angle  (rad) 

ThPd  =  Wd  *  DelThP  /  (Tf  -  TO);  %  Velocity  (rad/sec) 

ThPdd  =  Wdd  *  DelThP  /  (Tf  -  T0)A2;  %  Acceleration  (rad/seeA2) 
%  Save  for  plotting 
QRei'(6)  =  ThP, 

QdotRef(6)  =  ThPd; 

QddotRef(6)  =  ThPdd; 

%  Payload  center  of  mass  position,  velocity,  and  acceleration 
Xc  =  XPO  +  W  *  (XPf  -  XPO); 

Xcd  -  Wd  *  (XPf  -  XPO)  /  (Tf  -  TO); 

Xcdd  =  Wdd  *  (XPf  -  XPO)  /  (Tf  -  T0)A2; 

Yc  =  YPO  +  W  *  (YPf  -  YPO); 

Ycd  =  Wd  *  (YPf  -  YPO)  /  (Tf  -  TO); 

Ycdd  =  Wdd  *  (YPf  -  YPO)  /  (Tf  -  T0)A2; 

%  Save  for  plotting 
QRef(7)  =  Xc, 

QdotRef(7)  =  Xcd; 

QddotRef(7)  =  Xcdd; 

QRef(8)  =  Yc; 

QdotRef(8'  =  Ycd; 

QddotRef(8)  =  Ycdd; 

%  Payload  endpoint  coordinates;  Q\,  Qy,  P.\,  Py 
Qx  =  Xc  -  LcP  *  cos(ThP); 

Qy  =  Yc  -  LcP  11  sin(ThP); 

Px  =  Xc  +  (LP  -  LcP)  *  cos(ThP); 

Py  =  Yc  +  (LP  -  LcP)  *  sin(ThP); 


%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  Solve  for  Arm  Angles  Required  by  desired  path  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%% 

%%  LEFT  ARM  %% 

%%%%" )%%%%%% 

%  Elbow  is  left  of  line  from  arm  base  to  Q  (RQ) 

LSx  =  LO  *  cos(ThO  +  ThLO); 

LSy  =  LO  *  sin(ThO  +  ThLO); 

RQ  =  sqrt((Qx-LSx)A2+(Qy-LSy)A2);  %  Length  from  arm  base  to  Q 
Betal  =  atan2(Qy-LSy,Qx-LSx);  %  Angle  from  arm  base  to  RQ 

%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RQ  and  Link  L 1 
Num  =  L I A2  +  RQA2  -  L2A2; 

Den  =  2*  LI  *RQ; 

Beta2  =  acos(Num/Den);  %  Angle  from  RQ  to  Link  1 

ThL  I  =  (Beta  1  +  Beta2)  -  (ThO  +  ThLO);  %  Theta  L 1 
%  Use  law  of  cosines  to  find  the  interior  angle  at  the  elbow 
Num  =  L 1 A2  +  L2A2  -  RQA2; 

Den  =  2  *  LI  *  L2; 
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Beta3  =  acos(Num/Den); 

ThL2  =  -(pi-Beta3), 

%Save  for  plotting 
QRef(2)  =  ThLl; 

QRef(3)  =  ThL2; 

%%%%%%%%%%%% 

%%  RIGHT  ARM  %% 

%%%%%%%%%%%% 

%  Elbow  is  right  of  line  from  arm  base  (shoulder)  to  P  (wrist)  (RP) 

RSx  =  RO  *  cos(ThO  +  ThRO), 

RSv  =  RO  *  sin(ThO  +  ThRO); 

RP  =  sqrt((Px-RSx)A2+(Py-RSv)A2);  %  Length  from  arm  base  to  P 
Betal  =  atan2(Py-RSv,Px-RSx);  %  Angle  from  arm  base  to  RP 
%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RP  and  Link  R 1 
Num  =  RlA2  +  RPA2  -  R2A2; 

Den  =  2  *  R1  *  RP; 

Beta2  =  acos(Num/Den);  %  Angle  from  Link  R 1  to  RP 

Beta4  =  Beta  1  -  (ThO  +  ThRO); 

ThRl  =  -(Beta2  -  Beta4); 

Num  =  R1A2  +  R2A2  -  RPA2; 

Den  =  2  *  R1  *  R2; 

Beta3  =  acos(Num/Den); 

ThR2  =  pi  -  Beta3; 

%  Save  for  plotting 
QRef(4)  =  ThRl; 

QRef(5)  =  ThR2; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  Solve  for  Arm  Angle  Rates  &  Accelerations  required  by  desired  path  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%% 

%%  LEFT  ARM  %% 

%%%%%%%%%%% 

%  [Qxd;  Qyd]  =  [Hl]*ThOd  +  [H2]*Thd 
%  Qxd  &  Qyd  are  x  &  y  components  of  point  Q  inertial  velocitv. 

%  Thd  =  [ThLl dot;  ThL2dot| 

%  H  matrices  are  made  from  expressing  the  x  &  v  components  of  Q  in 
%  terms  of  LO,  ThO,  ThLO,  L 1 ,  ThL  1 ,  L2,  and  TiiI-2. 

%  Qx=L 0*cos(Th0+ThL0 )+L  1  *cos(ThO+ThLO+ThL  1  )+L2*cos(  IhO+ ... 

%  ThLO+ThL l+ThL2) 

%  Qy=LO*sin(ThO+ThLO)+L  1  *sin(ThO+ThLO+ThL  1  )+L2*sin(ThO+... 

%  ThLO+ThL l+ThL2) 

%  The  differentiation  of  these  equations  lead  to 
%  [Qxd;  Qyd]  =  [Hl]*ThOd  +  (H2]*Thd  which  can  be  solved  for  Thd 
Qxd  =  Xcd  +  LcP  *  ThPd  *  sin(ThP); 

Qyd  =  Ycd  -  LcP  *  ThPd  *  cos(ThP); 

H2(l,2)  =  -L2*sin(Th0+ThL0+ThL  l+ThL2); 

H2(l,l)  =  H2(  1 ,2)  -  L 1  *sin(ThO+ThLO+ThL  1 ); 

H2(2,2)  =  L  2*cos(Th0+ThL0+ThL  1  +ThL  2); 

1 12(2, 1 )  =  H2(2,2)  +  L 1  *cos(ThO+ThLO+ThL  1 ); 

HI (1,1)=  H2(l,l)  -  LO*sin(ThO+ThLO); 

II 1  (2, 1 )  =  H2(2, 1 )  +  LO*cos(ThO+ThLO); 

Thd  =  inv(H2)  *  ([Qxd;  Qyd]  -  Hl*ThOd); 

%  Angle  rates 
ThL  Id  =  Thd(l); 

ThL2d  =  Thd(2); 

%  Save  for  plotting 
QdotRef(2)  =  ThL  Id; 

QdotRef(3)  =  ThL2d; 
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%  Differentiation  of  [Qxd;  Qyd]  =  (Hl]*Th()d  +  (1 12|* Thd  leads  to 
%  IQxdd.  Qvdd|  =  l  H I  dot  1  *ThOd+|  H 1  ]  *Th(kld  +  |l  l2dot|*Thd+|M2|*Thdd 
gxdd  =  Xcdd  +  LcP*(ThPdd*sin(ThP)  +  ThPdA2*cos(  ITiP)); 
gvdd  =  Yedd  -  LcP*(ThPdd*cos(ThP)  -  ThPdA2*sin(ThP)); 

1 12dot(  l  ,2)  =  -L2*(Th0d+ThL  ld+ThL2d)*cos(  ThO+ThLO+ThL  1  +ThL2); 

I  I2dot(  1,1)=  H2dot(  1 ,2)  -  L 1  *(ThOd+  ThL  ld)*cos( ThO+ThLO+Thl.  1 ); 

I  !2dot(2,2)  =  -L2*(ThOd+ThL!d+ThL2d)*sin(ThO+ThLO+ThLl+ThL2); 

I  I2dol(2,l )  =  H2dot(2,2)  -  Ll*('rhOd+ThLld)*sin<  ThO+ThLO+ThL  1 ); 

I I  ldot(  1,1)=  1 12dot(  1,1)  -  LO*ThOd*cos( ThO+Thl.O); 

Illdot(2,l)  =  1 12dot(2, 1 )  -  LO*ThOd*sin(ThO+ThLO). 

fluid  =  inv(H2)*([Qxdd;  Qydd|-H2dot*Thd-(l  lldol|+ThOd-|l  1 1  |*Th()dd); 
%  Angle  accelerations 
ThL  Idd  =  Thdd(l); 

ThL2dd  =  Thdd(2), 
gddotRef(2)  =  ThL  ldd; 
gddotRef(3)  =  ThL2dd; 


%%%%%%%%%%%% 

%%  RIGHT  ARM  %% 

%%%%%%%%%%%% 

%  The  development  is  similar  to  the  left  arm 

%  Px=R()*cos(ThO+ThRO)+R  1  *cos(ThO+ThRO+lhR  1  )+R2*cos( Th()+ 

%  ThRO+ThR  1  +ThR2) 

%  Py=RO*sin(ThO+ThRO)+R  1  ♦sin(ThO+ThRO+ThR  1  )+R2*sin(ThO+ ... 

%  ThRO+ThR  l+ThR2) 

%  |Pxd,  Pvd|  =  |H1  |*ThOd  +  (H2)*Thd 
Pxd  =  Xcd  -  (LP  -  LcP)  *  ThPd  *  sin(ThP). 

Pvd  =  Ycd  +  (LP  -  LcP)  *  ThPd  *  cos(ThP). 
f  12(1,2)  =  -R2*sin(Th0+ThR0+ThR  l+ThR2); 

I12(  1 , 1 )  =  H2(  1 ,2)  -  R 1  *sin(ThO+ThRO+ThR  1 ); 

112(2,2)  =  R2*cos(ThO+ThRO+ThR  1  +ThR2), 

112(2, 1  )=  112(2,2)  +  R 1  *cos(ThO+ThRO+ThR  1 ); 

111(1,1)=  H2(l,l ) -  RO*sin(ThO+ThRO); 

1 11(2,1)  =  H2(2, 1 )  +  RO*cos(ThO+ThRO), 

Thd  =  inv(H2)  *  ([Pxd,  Pyd]  -  Hl*ThOd); 

%  Angle  rates 
ThRld  =  Thd(l); 

ThR2d  =  Thd(2); 

%  Save  for  plotting 
gdotRef(4)  =  ThRld; 
gdotRef(5)  =  ThR2d, 

%  [Pxdd;  Pydd)  =  [Hldot|*ThOd+[Hl]*ThOdd  +  [H2dot)*Thd+[H2)*Thdd 
Pxdd  =  Xcdd  -  (LP  -  LcP)*(ThPdd*sin(ThP)  +  ThPdA2*cos(ThP)); 

Pvdd  =  Ycdd  +  (LP  -  LcP)*(ThPdd*cos(ThP)  -  ThPdA2*sin(ThP)); 

H2dot(  1 ,2)  =  -R2*(ThOd+ThRld+ThR2d)*cos(ThO+ThRO+ThR  1  +ThR2); 
H2dot(l,l)=  H2dot(I,2)  -  Rl*(ThOd+ThRId)*cos(ThO+rhRO+ThRl). 

I  I2dot(2,2)  =  -R2*(ThOd+ThRld+ThR2d)*sin(rhO+ThRO+ThRl+ThR2); 
H2dot(2, 1 )  =  H2dot(2,2)  -  R I  ♦(ThOd+ThR  ld)*sin(ThO+ThRO+ThR  1 ), 

H 1  dot(  1,1)=  H2dot(  1,1)-  RO*ThOd*cos(ThO+ThRO). 

Hldot(2,l)  =  H2dot(2,l)  -  RO*ThOd*sm(ThO+ThRO); 

Thdd  =  inv(H2)*([Pxdd;  Pydd]-H2dot*Thd-[Hldot|*Th0d-[Hl]*Th0dd); 

%  Angle  accelerations 
ThRldd  =  Thdd(l); 

ThR2dd  =  Thdd(2); 
gddotRef(4)  =  ThRldd; 
gddotRef(5)  =  ThR2dd; 


%%%%%%%%%%%%%%%%%%% 
%%  Find  needed  control  torques,  u  %% 
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%%%%%%%%%%%%%%%%%%% 

%  EOM:  M*qddol  +  G  =  B*u  +  A'*Lam 

%  Constraint  Eqns.  A*qdot  =  0 
%  Solve  COM  for  qddot  leads  to 
%  qddot  =  mv(M)*(B*u  +  A'*Lam  -  G) 

%  Differentiate  Constraint  Eqns  gives  Adot*qdot  +  A*qddot  =  0 
%  Substitute  qddot  denved  from  EOM  into  differentiated  constraint 
%  eqns  and  solve  for  Lam 

%  Lam  =  -inv(A*inv(M)*A,)*(A*inv(M)*(B*u-G)+Adot*qdol> 

%  Substitute  this  expression  for  Lam  into  ongianl  COM.  Collect  terms 
%  into  the  form  MTilda’qddot  +  GTilda  =  BTilda*u 
%  where  MTilda  =  M 

%  GTilda  =  G  +  A'*inv(A*inv(M)*A‘)*(Adot*qdot-A*inv(M)*G) 

%  BTilda  =(I-A'*inv(A*inv(M)*A')*A*inv(M))*B 

%  The  first  five  resulting  equations  apply  to  the  spacecraft  centerbodv 
%  and  aims.  The  final  three  apply  to  the  payload.  The  matrix  form  of 
%  the  last  three  equations  is 
%  MPTilda*QPddot  +  GPTilda  =  BPTilda*u 

%%%%%%%%% 

%%  Matrices  %% 

%%%%%%%%% 

AngConst  =  [ThLO;  ThRO], 

%AMatFlag=  1, 
if  AMatFlag 

|  M,G,A,A<lot,B]  =  MatxFix(Ls,Ms,CMs,Is,QRef.QdotRef.  AngConst). 
else 

|M,G,A,Adot,Bl  =  Matx(Ls,Ms,CMs,Is,QRef,QdotRef,AngConst); 
end 

if  WheelFlag 

B7  =  [1;  0;  0;  0;  0;  0,0,0]; 

B  =  [B7  B]; 
end 

%  If  the  cost  function  is  subject  to  the  constraint  that  the  payload 
%  satisfy  the  reference  motion,  then  three  equations  of  motion  are  used. 

%  To  include  the  centerbody  reference  motion,  use  four  equations  from 
%  the  equations  of  motion. 


%%%%%%%%% 

%%  MTilda  %% 

%%%%%%%%% 

if  EOMFlag  —  3  %  Use  only  the  payload  equations 

MPTilda  =  M(6:8,6:8); 
else 

if  EOMFlag  ==  5  %  Use  the  spacecraft  equations 

MPTilda  =  M(  1:5, 1:5); 
else  %  Use  all  eight  equations 

MPTilda  =  M; 
end 
end 

%%%%%%%%% 

%%  GTilda  %% 

%%%%%%%%% 

Qdot  =  QdotRef , 

GTilda  =  G  +  A'*inv(A *inv(M)*A')*(Adot*Qdot  -  A*inv(M)*G), 
if  EOMFlag  —  3  %  Use  only  the  payload  equations 

GPTilda  =  GTilda(6:8,l); 
else 
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if  EOMFlag  ==  5  %  Use  the  spacecraft  equations 

GPTilda  =  GTildaf  1:5,1); 
else  %  Use  all  eight  equations 

GPTilda  =  G  Tilda, 
end 
end 

%%%%%%%%% 

%%  BTilda  %% 

%%%%%%%%% 

BTilda  =  (eye(8)  -  A’*inv(A*mv(M)*A')*A*m%,tM))*B. 
if  EOMFlag  =  3  %  Use  onlv  the  payload  equations 

BPTilda  =  BTilda(6:8,:); 
else 

if  EOMFlag  ==  5  %  Use  the  spacecraft  equations 

BPTilda  =  BTilda(  1:5,:), 
else  %  Use  all  eight  equations 

BPTilda  *  BTilda; 
end 
end 

%%%%%%%%% 

%%  01  &G2  %% 

%%%%%%%%% 

%  Use  previous  expression  for  Lam  and  regroup  terms  into  the  following 
%  form  A'*  Lam  =  K 1  +  K2*u 

K1  =  A"*inv(A*inv(M)*A')*(A*inv(M)*G-Adot*Qdot); 

K2  =  -A'*inv(A*inv(M)*A')*A*inv(M)*B; 

%%%%%%%%% 

%%  Torques  %% 

%%%%%%%%% 

%  Torques  are  calculated  to  minimize  the  following  cost  function. 

%  J  =  0  5*[u'*Wu*u  +  Lam'*A*Wc*A"*Lam  +  Tr*Wr*Tr] 

%  Subject  to  the  constraint:  MP*QPddot  +  GPTilda  -  BPTilda’u  -  0 
%  Combine  the  constraint  into  the  cost  function  by  multiplying  the 
%  constraint  eqn  by  another  Lagrange  multiplier.  Gam,  and  adding  that 
%  to  the  cost  function.  Take  the  gradient  with  respect  to  u  results  in 
%  (Wu+K2'*Wc*K2)*u  +  K2'*  Wc*K  1  -  BPTiIda'*Gam  =  0 
%  Solve  for  u 

%  u  =  inv(Wu+K2’*Wc*K2)*(BPTilda'*Gam  -  K2'*Wc*K  I ) 

%  Substitute  this  into  the  constraint  eqn.  Solve  the  result  for  Gam 
%  Gam  =  inv(BPTilda*inv(Wu  +  K2'*Wc*K2)*B!ynida’)*(MP*QPddot+ 
%  GPTilda+BPTilda*inv(Wu  +  K2'*We*K2)*K2'*Wc*KI) 

%  Substitute  this  expression  into  the  torque  equation,  u. 

Qddot  =  QddotRef ; 

if  EOMFlag  =  3  %  Use  onlv  the  payload  equations 

QPddot  =  Qddot(6:8, 1 ); 
else 

if  EOMFlag  =  5  %  Use  the  spacecraft  equations 

QPddot  =  Qddot(  1:5,1); 
else  %  Use  all  eight  equations 

QPddot  =  Qddot; 
end 
end 

%%%%%%%%%%%%%%%% 

%%  PSUEDO-INVERSES  %% 

%%%%%%%%%%%%%%%% 
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%  To  avoid  the  problems  with  poorly  conditioned  matrices.  I've  used  the 
%  psuedo-inverse  rather  than  the  traditional  inverse  in  the  next  two 
%  equations 
it  PInvFlag 

Parti  =  pinv(Wu  +  K2'*Wc*K2); 

tiam  =  pinv(BPTilda*Partl*BPTilda‘)  *  (MPTilda*gPddot  +  CiPTilda  + 
BPTiIda*Part  1  *(K2‘*  Wc*K  I )), 

else 

Parti  =  inv(Wu  +  K2'*Wc*K2); 

Cam  =  inv(BFrilda*Paitl*BPTilda’)  *  (MPTilda*QPddot  +  GPTilda  +.  . 
BlTilda*Part  1  ♦(K2'*Wc*K  1 )); 

end 

%  Reference  Torques 

Torques  =  Part  1  *(BPTilda'*Gam  -  K.2"*Wc*Kl): 

%  Cost  Function,  J 
.1  =  abs(Torques(  1 )); 

%Controller  Info 

Pt  1  =  A'*inv(A*inv(M)*A’). 

C I  =  inv(M)*(eye(M)  -  Ptl*A*inv(M))*B; 

C2  =  -inv(M)*Fitl*Adot; 

C3  =  inv(M)*(Ptl*A*inv(M)  -  eye(M))*G; 


%%%%%%%%%%%% 

%%  DEBUG  INFO  %% 

%%%%%%%%%%%% 

%%  Are  amstraint  equations.  A*qdol=0,  satisfied? 
Aqdot  =  A*QdotRef; 


L.  RefMin2 

%  Filename  is  'RefMin2.m' 

%  Reference  Maneuver  using  cost  function 

%  This  routine  is  used  by  "MainOpt.m"  to  find  the  optimal  combination 
%  of  reference  trajectory  polynomial  coefficients. 

%  Version  2  uses  the  rate  of  change  of  angular  momentum  to  find 
%  the  wheel  torque. 

function  [Joptl,Jopt2]  =  RefMin2(T,Ls,Ms,CMs,fs,BoundC,Wu,Wc.Coef,ConstMat) 
%  OUTPUTS: 

%  Jopt  =  absolute  value  of  the  reaction  wheel  torque.  This  is  the  cost 
%  function  value  for  purposes  of  optimizing  the  reference 
%  trajectory  polynomial  coefficients.  Jopt  1  will  be  integrated  by 
%  odemin.m  while  Jopt2  is  the  same  value  but  won’t  be  integrated. 

% 

%  INPUTS: 

%  T  =  time  (sec) 

%  Ls  =  7x  1  vector  of  lengths  (m) 

%  1  st  element  =  distance  from  origin  to  left  arm  mount 

%  2nd  &  3rd  elements  wrt  left  arm  (from  shoulder  toward  wrist) 

%  4th  element  =  payload  length 

%  5th  &  6th  elements  wrt  right  arm  (from  wrist  toward  shoulder) 

%  7th  element  =  distance  from  right  arm  mount  to  origin 

%  [L0;LI;L2;LP;R2;R1;R0] 

%  Ms  =  6x1  column  vector  containing  the  masses  (kg) 

%  1  st  element  =  mass  of  spacecraft  centerbody 
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%  2nd  &  3rd  elements  =  mass  of  left  arm  ( upper  arm  then  lower  arm ) 

%  4th  &  5th  elements  =  mass  of  right  arm  ( upper  arm  then  lower  aim ) 

%  6th  element  =  payload  mass 
%  | MO.  ML  1 ;  ML2,  MR  1 .  MR2;  MP| 

%  CMs  =  6x  1  column  vector  containing  center  of  mass  locations 
%  |LcO;  LcL  1;  LcL2:  LcRl.  LcR2;  LcP| 

%  Is  =  6x  1  column  vector  containing  the  moments  of  inertias  about  the 
%  respective  body's  center  of  mass  (kg  mA2) 

"  u  1  st  element  =  inertia  of  spacecraft  centcrbodv 

%  2nd  &  3rd  elements  =  inertia  of  left  arm  (upper  arm  then  lower  arm) 
%  4th  &  5th  elements  =  inertia  of  right  arm  (upper  arm  then  lower  arm) 
%  6th  element  =  payload  inertia 
%  1 10,  ILL.  1L2.  IR1,  1R2.  IP) 

BoundC  =  boundrv  conditions  for  the  problem  The  first  column 
%  contains  the  initial  x  and  y  component  of  points  0  &  P 
%  respectively,  the  x  component  of  the  right  arm  base,  the 
%  problem  start  time,  and  the  simulation  stop  time.  The  second 
%  column  contains  the  x  and  y  component  of  points  Q  &  P 
%  respectively,  the  x  component  of  the  right  arm  base,  the 
%  stop  time  for  the  ideal  reference  maneuver,  and  a  Hag  to 
%  activate  or  deactivate  the  controller  The  origin  for  the 
%  x  and  v  components  is  the  base  of  the  left  arm. 

%  Wu  =  6x6  control  torque  cost  weighting  matrix 
%  Wc  =  8x8  constraint  cost  weighting  matrix 

%  Coef  =  (n-2)x  1  vector  of  polynomial  reference  trajectory  coefficients 
%  in  descending  order  where  n  is  the  highest  order  coefficient 

%  ConstMat  =  3x(n-2)  matrix  of  coefficients  for  reference  tra  jectorv 
%  displacement  (row  1),  velocity  (row  2)  and  acceleration  (row  3) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  CONVERT  INPUTS  FROM  ARRAYS  TO  SCALARS  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Lengths  (m) 

1.0  =  Ls(l); 

LI  =  Ls(2); 

L2  =  Ls(3), 

LP  =  Ls(4); 

R2  =  Ls(5); 

R1  =  Ls(6); 

R0  =  Ls(7); 

%  Member  masses  (kg) 

MO  =  Ms(  1 ); 

ML1  =  Ms(2); 

ML2  =  Ms(3); 

MR  I  =  Ms(4); 

MR2  =  Ms(5), 

MP  =  Ms(6); 

%  Center  of  mass  distances  (m) 

LcO  =  CMs(l); 

LcLl  =CMs(2); 

I. cL2  =  CMs(3); 

LcRl  =  CMs(4); 

LcR2  =  CMs(5); 

LcP  =  CMs(6);  %measured  from  left  end 

%  MOI  about  center  of  mass 
10  =  Is(  1 ); 

II,  1  =  Is(2); 

IL2  =  Is(3); 
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IRI  =  ls(4); 

IR2  =  Is(5); 

IP  =  ls(6>; 

‘Mi  Initial  and  final  locations  of  third  link 
%  Point  Q  is  at  Node  3  (joint  between  Links  2  &  3) 

%  Point  P  is  at  Node  4  (joint  between  Links  3  &  4) 

QxO  =  BoundC(  1,1);  QyO  =  BoundC(  1 ,2), 

P.\0  =  BoundC(2,l );  PvO  =  BoundC(2,2), 

Qxf  =  BoundC(3,l);  Qyf  =  BoundC(3,2). 

P\f  =  BoundC(4, 1 ),  Pyf  =  BoundC(4,2). 

%  Arms  mount  locations  wrt  spacecraft  centcrhodv  coordinate  frame  (rad) 
fhl .0  =  BoundC(5, 1 );  .  hRO  =  BoundC(5,2); 

%  Reference  Maneuver  Start  and  Stop  Times 
TO  =  BoundC(6,l);  If  =  BoundC(6,2); 

%  Constraints  Matrix  Flag 
AMatFIag  =  BoundC(8,l); 

%  Centerbodv  Reaction  Wheel  Flag 
WheelFlag  =  BoundC(8,2); 

%  Centerbody  initial  and  Final  Conditions 
ThOO  =  BoundC(9, 1 ); 

ThOf  =  BoundC(9,2); 

%  Number  of  equations  in  the  cost  function  constraint  equations 
HOMFlag  =  BoundC(lO.l); 

%  Psuedo-Inverse  Flag 
PInvFlag  =  BoundC(10,2); 

%%%%%%%%%%%%%%%%%%%%%% 

%%  PRELIMINARY  CALCULATIONS  %% 
%%%%%%%%%%%%%%%%%%%%%% 

R2D  =  1 80/pi;  %  Conversion  from  radians  to  degrees 

%  Total  rotation  of  Payload 

ThPO  =  atar.2(Py0-Qy0,Px0-Qx0);  %  Initial  angle  of  Payload  (rad) 

ThPf  =  atan2(Pyf-Qyf,Pxf-Qxf);  %  Final  angle  of  Payload  (rad) 

DcIThP  =  ThPf  -  ThPO,  %  Total  delta  angle  of  Payload  (rad) 

%  Initial  and  final  locations  of  Payload  center  of  mass 
XPO  =  QxO  +  (PxO  -  QxO)  *  (LcP/LP); 

YPO  =  QyO  +  (PyO  -  QyO)  *  (LcP/LP); 

XPf  =  Qxf  +  (Pxf  -  Qxf)  *  (LcP/LP); 

YPf  =  Qyf  +  (Pyf  -  Qyf)  *  (LcP/LP); 

Tau  =  (T-TO)  /  (Tf-TO);  %  Normalize  time 

%  Function  Weighting  Factors  for  how  the  payload  will  move 
%  These  factors  will  cause  the  angular  velocity  and  angular 
%  acceleration  of  the  payload  to  be  zero  at  t  =  0  and  t  =  tf 
%  They  also  permit  the  payload  angle  to  match  its  initial 
%  and  final  values.  These  weighting  factors  will  also  apply 
%  to  the  translational  motion  of  the  payload  center  of  mass. 

k  =  length(Coef); 
forn=l  :k 
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CTau(k+l-n)  =  Coef(k+l-n)*TauA(n+2): 

<JTaud(k+l-n)  =  Coef(k+l-n)*TauA(n+l), 

Cfaudd(k+l-n)  =  Coef(k+l-n)*TauA(n), 
end 

W  =  ConstMaK  l,:)*CTau‘. 

Wd  =  (_'on.stMat(2,:)*CTaud'; 

Wdd  =  ConstMat(3 ,  )*CTaudd'. 

%  Centerbodv  angle,  angular  velocity,  angular  acceleration 
DelThO  =  ThOf  -  ThOO; 

IhO  =  ThOO  +  W  *  DelThO,  %  Angle  (rad), 

TliOd  =  Wd  *  DelThO  /  (Tf  -  TO);  %  Velocity  (rad/sec): 

TliOdd  =  Wdd  *  DelThO  /  (Tf  -  T0)A2;  %  Acceleration  (rad/seeA2). 

%  Save  for  plotting 
QRef(l)  =  ThO; 

QdolReffl)  =  ITiOd; 

QddolRef(l)  =  ThOdd; 

%  Payload  angle,  angular  velocity,  angular  acceleration 
ThP  =  ThPO  +  W  *  DelThP;  %  Angle  (rad) 

ThPd  =  Wd  *  DelThP  /  (Tf  -  TO);  %  Velocity  ( ratlVsec) 

ThPdd  =  Wdd  *  DelThP  /  (Tf  -  T0)A2,  %  Acceleration  (rad7secA2) 

%  Save  for  plotting 
QRcf(6)  =  ThP; 

QdotRcf(6)  =  ThPd, 

QddotRef(6)  =  ThPdd; 

%  Payload  center  of  mass  position,  velocity,  and  acceleration 
Xc  =  XPO  +  W  *  (XPf  -  XPO); 

Xcd  =  Wd  *  (XPf  -  XPO)  /  (Tf  -  TO); 

Xcdd  =  Wdd  *  (XPf  -  XPO)  /  (Tf  -  T0)A2; 

Yc  =  YPO  +  W  *  (YPf  -  YPO); 

Ycd  =  Wd  *  ( YPf  -  YPO)  /  (Tf  -  TO), 

Ycdd  =  Wdd  *  (YPf  -  YPO)  /  (Tf  -  T0)A2; 

%  Save  for  plotting 
QRef(7)  =  Xc; 

QdotRef(7)  =  Xcd, 

QddotRcf(7)  =  Xcdd; 

QRef(8)  =  Yc; 

QdotRef(8)  =  Ycd; 

QddotRef(8)  =  Ycdd; 

%  Payload  endpoint  coordinates.  Qx,  Qy,  Px,  Py 
Qx  =  Xc  -  LcP  *  cos(ThP); 

Qy  =  Yc  -  LcP  *  sin(ThP); 

Px  =  Xc  +  (LP  -  LcP)  *  cos(ThP); 

Py  =  Yc  +  (LP  -  LcP)  *  sin(ThP); 

%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  Solve  for  Arm  Angles  Required  by  desired  path  %% 
%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%% 

%%  LEFT  ARM  %% 

%%%%%%%%%%% 

%  Elbow  is  left  of  line  from  arm  base  to  Q  (RQ) 

LSx  =  LO  *  cos(ThO  +  ThLO); 

LSy  =  LO  *  sin(ThO  +  ThLO), 

RQ  =  sqrt((Qx-LSx)A2+(Qy-LSy)A2);  %  Length  from  arm  base  to  Q 
Betal  =  atan2(Qy-LSy,Qx-LSx);  %  Angle  from  arm  base  to  RQ 

%  Law  of  cosines;  cos(A)  =  (bA2  +cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RQ  and  Link  L 1 
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Nam  =  I.  IA2  +  RQA2  -  L2A2; 

IX*n  =  2  *  L I  *  RQ; 

Beta2  =  acos(Num/Den).  %  Angle  from  RQ  to  Link  1 

Thl.  I  =  (Betal  +  Bcla2)  -  (ThO  +  ThLO).  %  Theta  1. 1 
%  Use  law  of  cosines  to  find  the  interior  angle  at  the  elbow 
Num  =  1. 1A2  +  L2A2  -  RQA2; 

Den  =  2  *  LI  *  L2; 

Ileta.1  =  acos(Num/Den); 

ThL2  =  -(p>-Beta3); 

%Save  for  plotting 
QRef(2)  =  ThLl; 

QRef(3)  =  ThL2; 


%%%%%%%%%%%% 

%%  RIGHT  ARM  %% 

%%%%%%%%%%%% 

%  Hlbow  is  right  of  line  from  arm  base  (shoulder)  to  P  (wnst)  (RP) 
RSx  =  RO  *  cosfThO  +  ThRO). 

RSv  =  RO  *  sin(ThO  +  ThRO), 

RP  =  sqrt((Px-RSx)A2+(Py-RSy)A2);  %  Length  from  arm  base  to  P 
Betal  =  atan2(Py-RSy,Px-RSxj;  %  Angle  from  arm  base  to  RP 
%  Law  of  cosines:  cos(A)  =  (bA2  +  cA2  -  aA2)/(2bc) 

%  Apply  to  find  angle  between  RP  and  Link  R 1 
Num  =  R 1 A2  +  RPA2  -  R2A2; 

Den  =  2  *  R1  ♦  RP; 

Bcta2  =  acosfNum/Den),  %  Angle  from  Link  R1  to  RP 

Bcta4  =  Betal  -  (ThO  +  ThRO); 

ThR  1  =  -(Beta2  -  Beta4); 

Num  =  R 1 A2  +  R2A2  -  RPA2; 

Den  =  2  *  R1  *  R2; 

Beta3  =  acos(Num/Den); 

ThR2  =  pi  -  Beta3, 

%  Save  for  plotting 
QRef(4)  =  ThRl, 

QRef(5)  =  ThR2; 


%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  Solve  for  Arm  Angle  Rates  &  Accelerations  %% 

%%  required  by  desired  path  %% 

%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%% 

%%  LEFT  ARM  %% 

%%%%%%%%%%% 

%  (Qxd;  QydJ  =  (Hl]*ThOd  +  [H2]*Thd 
%  Qxd  &  Qyd  are  x  &  y  components  of  point  Q  inertial  velocity. 

%  Thd  =  [ThLl dot;  Thl2dot] 

%  H  matrices  are  made  from  expressing  the  x  &  y  components  of  Q  in 
%  terms  of  LO,  ThO,  ThLO,  LI,  ThLl,  L2,  and  ThL2. 

%  Qx=LO*cos(ThO+ThLO)+L  1  *cos(ThO+ThLO+ThL  1  )+L2*cos(ThO+  . . 
ThLO+ThL 1 +ThL2) 

%  Qy=LO*sin(ThO+ThLO)+L  1  *sin(ThO+ThLO+ThL  1  )+L2*sin(ThO+... 
ThLO+ThL l+ThL2) 

%  The  diiTerentiation  of  these  equations  lead  to 
%  [Qxd;  Qyd]  =  [Hl]*ThOd  +  [H2]*Thd  which  can  be  solved  for  Thd 
Qxd  =  Xcd  +  LcP  *  ThPd  *  sin(ThP); 

Qyd  =  Ycd  -  LcP  *  ThPd  *  cos(ThP); 

1 12(1,2)  =  -L2*sin(Th0+ThL0+ThL  l+ThL2); 

U2(  1 , 1 )  =  H2(  1 ,2)  -  L 1  *sin(ThO+ThLO+ThL  1 ); 

H2(2,2)  =  L2*cos(Th0+ThL0+ThL  1  +ThL2); 

H2(2, 1 )  =  H2(2,2)  +  L 1  *cos(ThO+ThLO+ThL  1 ), 

H 1  ( 1 , 1 )  =  H2(  1,1)  -  LO*sin(ThO+ThLO); 
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1 1 1  (2. 1 )  =  112(2,1)  +  1 ,0*cos(  I  hO+TliLO ). 

Ihd  =  mv(H2)  *  (|Qxd,  Qyd|  - 11 1  ♦Th(Xl). 

%  Angle  rates 
Till,  Id  =  Thd(l), 

Thl.2d  =  Thd(2); 

%  Save  lor  plotting 
gdotRef(2)  =  ThLld. 
gdolRe((3)  =  ThL2d. 

%  Differentiation  of  (Qxd:  Qvd|  =  (II I  |*Th(kl  +  |I  12[*'ITid  leads  to 
%  |Q\dd:  Qydd|  =  (Hldot|*iTiOd+lHI)*Th()dd  +  )H2dot|*Thd+|II21*Thdd 
Q\dd  =  Xedd  +  LcP*(ThPdd*sin(  ThP)  +  ThPdA2*cos<ThP)); 
gvdd  =  Yedd  -  LcP*(ThPdd*cos(ThP)  -  ThPdA2*sin<ThP)); 

I  !2dot(  1 ,2)  =  -L2*(Th0d-t-ThL  ld+Thl.2d)*cos(ll)0+Thl.0+rhl.  I  +TW.2): 

1 12dot(  1,1)  =  H2dot(  1 ,2)  - 1,1  *(ThOd+  lhl .  1  d)*eost  ThO+  1’hl -O+Thl .  I ). 

I  !2dot(2,2)  =  -l-2*(ThOd+ThLld+ThL2d)*sin(  ThO+l  hl-O+ThL  1  +  l  hl.2). 

1 1 2dot( 2,1)=  H2dot(2,2)  -  L 1  *(ThOd+ThL  1  d)*sin( ThO+ThI .O+Thl .  1 ). 

1 1 1  dot(  1,1)=  H2dot(  1,1)-  I-0*Th0d*cos( ThO+Thl.O); 

Hldot(2,l)  =  H2dot(2, 1 )  -  LO*ThOd*sin(Th(HThl.U). 

Thdd  =  inv(H2)*((Qxdd.  Qydd|-H2dot*Thd-[Hldot|*Th(  d-|Ill|*Th0dd). 
%  Angle  accelerations 
Thl-ldd  =  Thdd(l); 

Thl.2dd  =  Thdd(2); 
gddotRef(2)  =  ThL  ldd; 

OJdotRel'(3)  =  ThL2dd; 

%%%%%%%%%%%% 

%%  RIGHT  ARM  %% 

%%%%%%%%%%%% 

%  The  development  is  similar  to  the  left  arm 

%  Px=RO*cos(ThO+ThRO)+R  1  *cos(ThO+ThRO+ThR  1  )+R2*cos(ThO+ 
ThRO+ThR  1  +ThR2) 

%  Pv=RO*sin(ThO+ThRO)+R  1  *sin(ThO+ThRO+ThR  1  )+R2*sin(  Th()+  . 

ThRO+ThR  l+ThR2) 

%  [Pxd;  Pyd]  =  (Hl]*ThOd  +  [H2J*Thd 
Pxd  =  Xcd  -  (LP  -  LcP)  *  ThPd  *  sin(ThP), 

Pvd  =  Ycd  +  (LP  -  LcP)  *  ThPd  *  cos(ThP); 

112(1,2)  =  -R2*sin(ThO+ThRO+ThRl+ThR2), 

112(1,1)  -  H2(  1 ,2)  -  R 1  *sin(ThO+ThRO+ThR  1 ). 

1 12(2,2)  =  R2*cos(Th0+ThR0+ThR  1  +ThR2); 

1 12(2, 1 )  =  H2(2,2)  +  R 1  *cos(ThO+ThRO+ ThR  1 ); 

111(1,1)  =  H2(l,l)  -  RO*sin(ThO+ThRO); 

111(2,1)=  H2(2, 1 )  +  RO*cos(ThO+ThRO)', 

I'hd  =  inv(H2)  *  ((Pxd;  Pyd]  -  HI*ThOd); 

%  Angle  rates 
ThRId  =  Thd(I), 

ThR2d  =  Thd(2); 

%  Save  for  plotting 
gdotRef(4)  =  ThRId, 
gdotRef(5)  =  ThR2d; 

%  (Pxdd;  Pydd]  =  (Hldot]*ThOd+(Hl}*ThOdd  +  |lI2dot|*Thd+(112j*Thdd 
Pxdd  =  Xcdd  -  (LP  -  LcP)*(ThPdd*sin('rhP)  +  ThPdA2*cos(ThP)); 

Pvdd  =  Ycdd  +  (LP  -  LcP)*(ThPdd*cos(ThP)  -  ThPdA2*sm(ThP)); 
H2dot(l,2)  =  -R2*(Th0d+ThR  ld+ThR2d)*cos(ThO+ThRO+ThR  1  +ThR2); 
H2dot(  1,D=  H2dot(  1 ,2)  -  R 1  *{ThOd+ThR  1  d)*cos(ThO+ThRO+ThR  I ); 
H2dot(2,2)  =  -R2*(ThOd+ThRld+ThR2d)*sin(ThO+ThRO+ThRl+ThR2); 
H2dot(2,l)  =  H2dot(2,2)  -  R 1  *(ThOd+ThR  1  d)*sin('I'hO+ThRO+ThR I ): 

H 1  dot(  1,1)=  H2dot(  1,1)-  RO*ThOd*cos(  ITiO+lhRO); 

Hldot(2,l)  =  H2dot(2,l)  -  RO*ThOd*sin(ThO+ThRO), 

Thdd  =  inv(H2)*([Pxdd;  Pydd]-H2dot*Thd-(Hldot]',Th0d-[Hl]*Th0dd); 


Angle  accelerations 
IhRldd  =  Thdd(l), 
lhR2dd  =  Thdd<2). 

<JddolRef(4)  =  IhRldd. 
oddoiRet(5)  =  ThR2dd. 

'■i.%%%%%%%%%%%%%%%%%%% 

"t.%  l  ind  needed  control  wheel  torque  %% 
■’;,%%%%%%%%%%%%%%%%%%% 

Q  =  QR.r. 

Odot  =  <v  iolRef. 

(Jddot  =  (jddolRel’, 

|l  Is.  l(dots|  =  AngMo2(Ls,Ms,C'Ms,ls,Q,Qdot.yddot). 

"I.  Cost  function,  Jopt 

'  »  Wheel  torque  is  the  change  in  total  angular  momentum 
%  Jopt  I  is  integrated  while  Jopt  2  is  not 
jopt  1  =  abs(Hdots(7)), 

Jopt2  =  Jopt  1 ; 
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